From d6ac8a72cabd5b0bb29735eb33087f6e01d70ea6 Mon Sep 17 00:00:00 2001 From: frank Date: Mon, 6 Aug 2018 18:33:17 +0200 Subject: [PATCH] worked on fir/iir filters --- math/dsp/fir/Complex.h | 12 +- math/dsp/fir/Real.h | 15 +- math/dsp/iir/BiQuad.h | 274 +++++++++++++------------ sensors/imu/PoseDetection.h | 344 +++----------------------------- sensors/imu/PoseDetectionPlot.h | 13 ++ 5 files changed, 216 insertions(+), 442 deletions(-) diff --git a/math/dsp/fir/Complex.h b/math/dsp/fir/Complex.h index 513ef6a..a194e13 100644 --- a/math/dsp/fir/Complex.h +++ b/math/dsp/fir/Complex.h @@ -181,8 +181,16 @@ private: } // https://dsp.stackexchange.com/questions/4693/fir-filter-gain - static void normalizeAC(std::vector>& kernel, const float freq) { - throw std::runtime_error("TODO"); + static void normalizeAC(std::vector>& kernel, const float freq, const float sRate) { +// std::complex sum; +// for (size_t i = 0; i < kernel.size(); ++i) { +// const float t = (float) i / sRate; +// const float v = std::sin(t*freq); + +// } +// for (auto f : kernel) {sum += f * sin;} +// for (auto& f : kernel) {f /= sum;} + throw std::runtime_error("todo"); } /** build a lowpass filter kernel */ diff --git a/math/dsp/fir/Real.h b/math/dsp/fir/Real.h index cae949c..c7eec85 100644 --- a/math/dsp/fir/Real.h +++ b/math/dsp/fir/Real.h @@ -35,6 +35,16 @@ namespace FIR { this->kernel = kernel; } + const Kernel& getKernel() const { + return this->kernel; + } + + void prefill(float val) { + for (size_t i = 0; i < (kernel.size()-1)/2; ++i) { + append(val); + } + } + /** filter the given incoming real data */ DataVec append(const DataVec& newData) { // append to local buffer (as we need some history) @@ -56,10 +66,11 @@ namespace FIR { DataVec processLocalBuffer() { // sanity check - Assert::isNot0(kernel.size(), "FIRComplex:: kernel not yet configured!"); + Assert::isNot0(kernel.size(), "FIR::Real::Filter kernel not yet configured!"); // number of processable elements (due to filter size) - const int processable = data.size() - kernel.size() + 1 - kernel.size()/2; + //const int processable = data.size() - kernel.size() + 1 - kernel.size()/2; + const int processable = data.size() - kernel.size(); // nothing to-do? if (processable <= 0) {return DataVec();} diff --git a/math/dsp/iir/BiQuad.h b/math/dsp/iir/BiQuad.h index 39bd679..67aee40 100644 --- a/math/dsp/iir/BiQuad.h +++ b/math/dsp/iir/BiQuad.h @@ -46,6 +46,14 @@ namespace IIR { } + float getA0() {return 1;} + float getA1() {return a1a0;} + float getA2() {return a2a0;} + + float getB0() {return b0a0;} + float getB1() {return b1a0;} + float getB2() {return b2a0;} + void preFill(const Scalar s) { for (int i = 0; i < 100; ++i) { filter(s); @@ -67,12 +75,14 @@ namespace IIR { } /** configure the filter as low-pass. freqFact between ]0;0.5[ */ - void setLowPass( double freqFact, const float octaves ) { + //void setLowPass( double freqFact, const float octaves ) { + void setLowPass( double freqFact, const float Q ) { sanityCheck(freqFact); double w0 = 2.0 * M_PI * freqFact; - double alpha = sin(w0)*sinh( log(2)/2 * octaves * w0/sin(w0) ); + //double alpha = sin(w0)*sinh( log(2)/2 * octaves * w0/sin(w0) ); + double alpha = sin(w0)/(2*Q); double b0 = (1.0 - cos(w0))/2.0; double b1 = 1.0 - cos(w0); @@ -94,46 +104,48 @@ namespace IIR { - //http://dspwiki.com/index.php?title=Lowpass_Resonant_Biquad_Filter - //http://www.opensource.apple.com/source/WebCore/WebCore-7536.26.14/platform/audio/Biquad.cpp - /** - * configure as low-pass filter with resonance - * @param freqFact the frequency factor between ]0;0.5[ - * @param res - */ - void setLowPassResonance( double freqFact, float res ) { +// //http://dspwiki.com/index.php?title=Lowpass_Resonant_Biquad_Filter +// //http://www.opensource.apple.com/source/WebCore/WebCore-7536.26.14/platform/audio/Biquad.cpp +// /** +// * configure as low-pass filter with resonance +// * @param freqFact the frequency factor between ]0;0.5[ +// * @param res +// */ +// void setLowPassResonance( double freqFact, float res ) { - sanityCheck(freqFact); +// sanityCheck(freqFact); - res *= 10; +// res *= 10; - double g = pow(10.0, 0.05 * res); - double d = sqrt((4 - sqrt(16 - 16 / (g * g))) / 2); +// double g = pow(10.0, 0.05 * res); +// double d = sqrt((4 - sqrt(16 - 16 / (g * g))) / 2); - double theta = M_PI * freqFact; - double sn = 0.5 * d * sin(theta); - double beta = 0.5 * (1 - sn) / (1 + sn); - double gamma = (0.5 + beta) * cos(theta); - double alpha = 0.25 * (0.5 + beta - gamma); +// double theta = M_PI * freqFact; +// double sn = 0.5 * d * sin(theta); +// double beta = 0.5 * (1 - sn) / (1 + sn); +// double gamma = (0.5 + beta) * cos(theta); +// double alpha = 0.25 * (0.5 + beta - gamma); - double a0 = 1.0; - double b0 = 2.0 * alpha; - double b1 = 2.0 * 2.0 * alpha; - double b2 = 2.0 * alpha; - double a1 = 2.0 * -gamma; - double a2 = 2.0 * beta; +// double a0 = 1.0; +// double b0 = 2.0 * alpha; +// double b1 = 2.0 * 2.0 * alpha; +// double b2 = 2.0 * alpha; +// double a1 = 2.0 * -gamma; +// double a2 = 2.0 * beta; - setValues(a0, a1, a2, b0, b1, b2); +// setValues(a0, a1, a2, b0, b1, b2); - } +// } /** configure the filter as high-pass. freqFact between ]0;0.5[ */ - void setHighPass( double freqFact, const float octaves ) { + //void setHighPass( double freqFact, const float octaves ) { + void setHighPass( double freqFact, const float Q ) { sanityCheck(freqFact); double w0 = 2.0 * M_PI * freqFact; - double alpha = sin(w0)*sinh( log(2)/2 * octaves * w0/sin(w0) ); + //double alpha = sin(w0)*sinh( log(2)/2 * octaves * w0/sin(w0) ); + double alpha = sin(w0)/(2*Q); double b0 = (1.0 + cos(w0))/2.0; double b1 = -(1.0 + cos(w0)); @@ -153,17 +165,21 @@ namespace IIR { } /** configure the filter as band-pass. freqFact between ]0;0.5[ */ - void setBandPass( double freqFact, const float octaves ) { + //void setBandPass( double freqFact, const float octaves ) { + void setBandPass( double freqFact, const float Q ) { sanityCheck(freqFact); //double w0 = 2 * K_PI * / 2 / freqFact; double w0 = 2.0 * M_PI * freqFact; - double alpha = sin(w0)*sinh( log(2)/2 * octaves * w0/sin(w0) ); + //double alpha = sin(w0)*sinh( log(2)/2 * octaves * w0/sin(w0) ); + double alpha = sin(w0)/(2*Q); - double b0 = sin(w0)/2.0; + + // constant 0dB peak gain + double b0 = alpha; double b1 = 0.0; - double b2 = -sin(w0)/2.0; + double b2 = -alpha; double a0 = 1.0 + alpha; double a1 = -2.0*cos(w0); double a2 = 1.0 - alpha; @@ -179,108 +195,112 @@ namespace IIR { } - /** configure the filter as all-pass. freqFact between ]0;0.5[ */ - void setAllPass( double freqFact, const float octaves ) { +// /** configure the filter as all-pass. freqFact between ]0;0.5[ */ +// void setAllPass( double freqFact, const float octaves ) { - sanityCheck(freqFact); +// sanityCheck(freqFact); - double w0 = 2.0 * M_PI * freqFact; - double alpha = sin(w0)*sinh( log(2)/2 * octaves * w0/sin(w0) ); +// double w0 = 2.0 * M_PI * freqFact; +// double alpha = sin(w0)*sinh( log(2)/2 * octaves * w0/sin(w0) ); - double b0 = 1 - alpha; - double b1 = -2*cos(w0); - double b2 = 1 + alpha; - double a0 = 1 + alpha; - double a1 = -2*cos(w0); - double a2 = 1 - alpha; +// double b0 = 1 - alpha; +// double b1 = -2*cos(w0); +// double b2 = 1 + alpha; +// double a0 = 1 + alpha; +// double a1 = -2*cos(w0); +// double a2 = 1 - alpha; - setValues(a0, a1, a2, b0, b1, b2); +// setValues(a0, a1, a2, b0, b1, b2); - } +// } - /** configure the filter as all-pass */ - void setAllPass( const float freq, const float octaves, const float sRate ) { - double freqFact = double(freq) / double(sRate); - setAllPass(freqFact, octaves); - } - - /** configure as notch filter. freqFact between ]0;0.5[ */ - void setNotch( double freqFact, const float octaves ) { - - sanityCheck(freqFact); - - double w0 = 2.0 * M_PI * freqFact; - double alpha = sin(w0)*sinh( log(2)/2 * octaves * w0/sin(w0) ); - - double b0 = 1.0; - double b1 = -2.0*cos(w0); - double b2 = 1.0; - double a0 = 1.0 + alpha; - double a1 = -2.0*cos(w0); - double a2 = 1.0 - alpha; - - setValues(a0, a1, a2, b0, b1, b2); - - } - - /** configure as notch filter */ - void setNotch( const float freq, const float octaves, const float sRate ) { - double freqFact = double(freq) / double(sRate); - setNotch(freqFact, octaves); - } - - /** configure the filter as low-shelf. increase all aplitudes below freq? freqFact between ]0;0.5[ */ - void setLowShelf( double freqFact, const float octaves, const float gain ) { - - sanityCheck(freqFact); - - double A = sqrt( pow(10, (gain/20.0)) ); - double w0 = 2.0 * M_PI * freqFact; - double alpha = sin(w0)*sinh( log(2)/2 * octaves * w0/sin(w0) ); - - double b0 = A*( (A+1.0) - (A-1.0)*cos(w0) + 2.0*sqrt(A)*alpha ); - double b1 = 2.0*A*( (A-1.0) - (A+1.0)*cos(w0) ); - double b2 = A*( (A+1.0) - (A-1.0)*cos(w0) - 2.0*sqrt(A)*alpha ); - double a0 = (A+1.0) + (A-1.0)*cos(w0) + 2.0*sqrt(A)*alpha; - double a1 = -2.0*( (A-1.0) + (A+1.0)*cos(w0) ); - double a2 = (A+1.0) + (A-1.0)*cos(w0) - 2.0*sqrt(A)*alpha; - - setValues(a0, a1, a2, b0, b1, b2); - - } - - /** configure the filter as low-shelf. increase all aplitudes below freq? */ - void setLowShelf( const float freq, const float octaves, const float gain, const float sRate ) { - double freqFact = double(freq) / double(sRate); - setLowShelf(freqFact, octaves, gain); - } - - /** configure the filter as high-shelf. increase all amplitues above freq? freqFact between ]0;0.5[ */ - void setHighShelf( double freqFact, const float octaves, const float gain ) { - - sanityCheck(freqFact); - - double A = sqrt( pow(10, (gain/20.0)) ); - double w0 = 2.0 * M_PI * freqFact; - double alpha = sin(w0)*sinh( log(2)/2 * octaves * w0/sin(w0) ); - - double b0 = A*( (A+1.0) + (A-1.0)*cos(w0) + 2.0*sqrt(A)*alpha ); - double b1 = -2.0*A*( (A-1.0) + (A+1.0)*cos(w0) ); - double b2 = A*( (A+1.0) + (A-1.0)*cos(w0) - 2.0*sqrt(A)*alpha ); - double a0 = (A+1.0) - (A-1.0)*cos(w0) + 2.0*sqrt(A)*alpha; - double a1 = 2.0*( (A-1.0) - (A+1.0)*cos(w0) ); - double a2 = (A+1.0) - (A-1.0)*cos(w0) - 2.0*sqrt(A)*alpha; - - setValues(a0, a1, a2, b0, b1, b2); - - } +// /** configure the filter as all-pass */ +// void setAllPass( const float freq, const float octaves, const float sRate ) { +// double freqFact = double(freq) / double(sRate); +// setAllPass(freqFact, octaves); +// } - /** configure the filter as high-shelf. increase all amplitues above freq? */ - void setHighShelf( const float freq, const float octaves, const float gain, const float sRate ) { - double freqFact = double(freq) / double(sRate); - setHighShelf(freqFact, octaves, gain); - } + + +// /** configure as notch filter. freqFact between ]0;0.5[ */ +// //void setNotch( double freqFact, const float octaves ) { +// void setNotch( double freqFact, const float Q ) { + +// sanityCheck(freqFact); + +// double w0 = 2.0 * M_PI * freqFact; +// double alpha = sin(w0)*sinh( log(2)/2 * octaves * w0/sin(w0) ); + +// double b0 = 1.0; +// double b1 = -2.0*cos(w0); +// double b2 = 1.0; +// double a0 = 1.0 + alpha; +// double a1 = -2.0*cos(w0); +// double a2 = 1.0 - alpha; + +// setValues(a0, a1, a2, b0, b1, b2); + +// } + +// /** configure as notch filter */ +// void setNotch( const float freq, const float octaves, const float sRate ) { +// double freqFact = double(freq) / double(sRate); +// setNotch(freqFact, octaves); +// } + +// /** configure the filter as low-shelf. increase all aplitudes below freq? freqFact between ]0;0.5[ */ +// void setLowShelf( double freqFact, const float octaves, const float gain ) { + +// sanityCheck(freqFact); + +// double A = sqrt( pow(10, (gain/20.0)) ); +// double w0 = 2.0 * M_PI * freqFact; +// double alpha = sin(w0)*sinh( log(2)/2 * octaves * w0/sin(w0) ); + +// double b0 = A*( (A+1.0) - (A-1.0)*cos(w0) + 2.0*sqrt(A)*alpha ); +// double b1 = 2.0*A*( (A-1.0) - (A+1.0)*cos(w0) ); +// double b2 = A*( (A+1.0) - (A-1.0)*cos(w0) - 2.0*sqrt(A)*alpha ); +// double a0 = (A+1.0) + (A-1.0)*cos(w0) + 2.0*sqrt(A)*alpha; +// double a1 = -2.0*( (A-1.0) + (A+1.0)*cos(w0) ); +// double a2 = (A+1.0) + (A-1.0)*cos(w0) - 2.0*sqrt(A)*alpha; + +// setValues(a0, a1, a2, b0, b1, b2); + +// } + +// /** configure the filter as low-shelf. increase all aplitudes below freq? */ +// void setLowShelf( const float freq, const float octaves, const float gain, const float sRate ) { +// double freqFact = double(freq) / double(sRate); +// setLowShelf(freqFact, octaves, gain); +// } + +// /** configure the filter as high-shelf. increase all amplitues above freq? freqFact between ]0;0.5[ */ +// void setHighShelf( double freqFact, const float octaves, const float gain ) { + +// sanityCheck(freqFact); + +// double A = sqrt( pow(10, (gain/20.0)) ); +// double w0 = 2.0 * M_PI * freqFact; +// double alpha = sin(w0)*sinh( log(2)/2 * octaves * w0/sin(w0) ); + +// double b0 = A*( (A+1.0) + (A-1.0)*cos(w0) + 2.0*sqrt(A)*alpha ); +// double b1 = -2.0*A*( (A-1.0) + (A+1.0)*cos(w0) ); +// double b2 = A*( (A+1.0) + (A-1.0)*cos(w0) - 2.0*sqrt(A)*alpha ); +// double a0 = (A+1.0) - (A-1.0)*cos(w0) + 2.0*sqrt(A)*alpha; +// double a1 = 2.0*( (A-1.0) - (A+1.0)*cos(w0) ); +// double a2 = (A+1.0) - (A-1.0)*cos(w0) - 2.0*sqrt(A)*alpha; + +// setValues(a0, a1, a2, b0, b1, b2); + +// } + + +// /** configure the filter as high-shelf. increase all amplitues above freq? */ +// void setHighShelf( const float freq, const float octaves, const float gain, const float sRate ) { +// double freqFact = double(freq) / double(sRate); +// setHighShelf(freqFact, octaves, gain); +// } protected: diff --git a/sensors/imu/PoseDetection.h b/sensors/imu/PoseDetection.h index c3c0770..968c3f1 100644 --- a/sensors/imu/PoseDetection.h +++ b/sensors/imu/PoseDetection.h @@ -21,61 +21,6 @@ */ 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 { @@ -127,52 +72,52 @@ class PoseDetection { }; - /** live-pose-estimation using moving median of the smartphone's accelerometer */ - struct EstMovingMedian { +// /** live-pose-estimation using moving median of the smartphone's accelerometer */ +// struct EstMovingMedian { - // median the accelerometer - MovingMedianTS medianX; - MovingMedianTS medianY; - MovingMedianTS medianZ; +// // median the accelerometer +// MovingMedianTS medianX; +// MovingMedianTS medianY; +// MovingMedianTS medianZ; - EstMovingMedian(const Timestamp window) : - medianX(window), medianY(window), medianZ(window) { +// EstMovingMedian(const Timestamp window) : +// medianX(window), medianY(window), medianZ(window) { - // start approximately - addAcc(Timestamp(), AccelerometerData(0,0,9.81)); +// // 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); - } +// /** 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()); - } +// AccelerometerData getBase() const { +// return AccelerometerData(medianX.get(), medianY.get(), medianZ.get()); +// } - /** get the current rotation matrix estimation */ - //Eigen::Matrix3f get() const { - Matrix3 get() const { +// /** get the current rotation matrix estimation */ +// //Eigen::Matrix3f get() const { +// Matrix3 get() const { - const Vector3 base(medianX.get(), medianY.get(), medianZ.get()); +// 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); +// // 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"); +// // 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; +// return rotMat; - } +// } - }; +// }; @@ -202,11 +147,6 @@ public: ; } -// /** 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; @@ -236,22 +176,6 @@ public: 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) { @@ -271,208 +195,6 @@ public: } - - - - -// /** 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(); - - }; diff --git a/sensors/imu/PoseDetectionPlot.h b/sensors/imu/PoseDetectionPlot.h index 74aae10..46b0d8f 100644 --- a/sensors/imu/PoseDetectionPlot.h +++ b/sensors/imu/PoseDetectionPlot.h @@ -111,6 +111,19 @@ } } +// // update un-rotated 3D smartphone model +// for (size_t i = 0; i < pose.size(); ++i) { +// K::GnuplotObjectPolygon* gp = (K::GnuplotObjectPolygon*) plotPose.getObjects().get(i+1); gp->clear(); +// for (const std::vector& 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);