worked on fir/iir filters
This commit is contained in:
@@ -181,8 +181,16 @@ private:
|
|||||||
}
|
}
|
||||||
|
|
||||||
// https://dsp.stackexchange.com/questions/4693/fir-filter-gain
|
// https://dsp.stackexchange.com/questions/4693/fir-filter-gain
|
||||||
static void normalizeAC(std::vector<std::complex<float>>& kernel, const float freq) {
|
static void normalizeAC(std::vector<std::complex<float>>& kernel, const float freq, const float sRate) {
|
||||||
throw std::runtime_error("TODO");
|
// std::complex<float> 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 */
|
/** build a lowpass filter kernel */
|
||||||
|
|||||||
@@ -35,6 +35,16 @@ namespace FIR {
|
|||||||
this->kernel = kernel;
|
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 */
|
/** filter the given incoming real data */
|
||||||
DataVec append(const DataVec& newData) {
|
DataVec append(const DataVec& newData) {
|
||||||
// append to local buffer (as we need some history)
|
// append to local buffer (as we need some history)
|
||||||
@@ -56,10 +66,11 @@ namespace FIR {
|
|||||||
DataVec processLocalBuffer() {
|
DataVec processLocalBuffer() {
|
||||||
|
|
||||||
// sanity check
|
// 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)
|
// 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?
|
// nothing to-do?
|
||||||
if (processable <= 0) {return DataVec();}
|
if (processable <= 0) {return DataVec();}
|
||||||
|
|||||||
@@ -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) {
|
void preFill(const Scalar s) {
|
||||||
for (int i = 0; i < 100; ++i) {
|
for (int i = 0; i < 100; ++i) {
|
||||||
filter(s);
|
filter(s);
|
||||||
@@ -67,12 +75,14 @@ namespace IIR {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/** configure the filter as low-pass. freqFact between ]0;0.5[ */
|
/** 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);
|
sanityCheck(freqFact);
|
||||||
|
|
||||||
double w0 = 2.0 * M_PI * 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 b0 = (1.0 - cos(w0))/2.0;
|
||||||
double b1 = 1.0 - cos(w0);
|
double b1 = 1.0 - cos(w0);
|
||||||
@@ -94,46 +104,48 @@ namespace IIR {
|
|||||||
|
|
||||||
|
|
||||||
|
|
||||||
//http://dspwiki.com/index.php?title=Lowpass_Resonant_Biquad_Filter
|
// //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
|
// //http://www.opensource.apple.com/source/WebCore/WebCore-7536.26.14/platform/audio/Biquad.cpp
|
||||||
/**
|
// /**
|
||||||
* configure as low-pass filter with resonance
|
// * configure as low-pass filter with resonance
|
||||||
* @param freqFact the frequency factor between ]0;0.5[
|
// * @param freqFact the frequency factor between ]0;0.5[
|
||||||
* @param res
|
// * @param res
|
||||||
*/
|
// */
|
||||||
void setLowPassResonance( double freqFact, float res ) {
|
// void setLowPassResonance( double freqFact, float res ) {
|
||||||
|
|
||||||
sanityCheck(freqFact);
|
// sanityCheck(freqFact);
|
||||||
|
|
||||||
res *= 10;
|
// res *= 10;
|
||||||
|
|
||||||
double g = pow(10.0, 0.05 * res);
|
// double g = pow(10.0, 0.05 * res);
|
||||||
double d = sqrt((4 - sqrt(16 - 16 / (g * g))) / 2);
|
// double d = sqrt((4 - sqrt(16 - 16 / (g * g))) / 2);
|
||||||
|
|
||||||
double theta = M_PI * freqFact;
|
// double theta = M_PI * freqFact;
|
||||||
double sn = 0.5 * d * sin(theta);
|
// double sn = 0.5 * d * sin(theta);
|
||||||
double beta = 0.5 * (1 - sn) / (1 + sn);
|
// double beta = 0.5 * (1 - sn) / (1 + sn);
|
||||||
double gamma = (0.5 + beta) * cos(theta);
|
// double gamma = (0.5 + beta) * cos(theta);
|
||||||
double alpha = 0.25 * (0.5 + beta - gamma);
|
// double alpha = 0.25 * (0.5 + beta - gamma);
|
||||||
|
|
||||||
double a0 = 1.0;
|
// double a0 = 1.0;
|
||||||
double b0 = 2.0 * alpha;
|
// double b0 = 2.0 * alpha;
|
||||||
double b1 = 2.0 * 2.0 * alpha;
|
// double b1 = 2.0 * 2.0 * alpha;
|
||||||
double b2 = 2.0 * alpha;
|
// double b2 = 2.0 * alpha;
|
||||||
double a1 = 2.0 * -gamma;
|
// double a1 = 2.0 * -gamma;
|
||||||
double a2 = 2.0 * beta;
|
// 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[ */
|
/** 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);
|
sanityCheck(freqFact);
|
||||||
|
|
||||||
double w0 = 2.0 * M_PI * 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 b0 = (1.0 + cos(w0))/2.0;
|
||||||
double b1 = -(1.0 + cos(w0));
|
double b1 = -(1.0 + cos(w0));
|
||||||
@@ -153,17 +165,21 @@ namespace IIR {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/** configure the filter as band-pass. freqFact between ]0;0.5[ */
|
/** 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);
|
sanityCheck(freqFact);
|
||||||
|
|
||||||
//double w0 = 2 * K_PI * / 2 / freqFact;
|
//double w0 = 2 * K_PI * / 2 / freqFact;
|
||||||
double w0 = 2.0 * M_PI * 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 b1 = 0.0;
|
||||||
double b2 = -sin(w0)/2.0;
|
double b2 = -alpha;
|
||||||
double a0 = 1.0 + alpha;
|
double a0 = 1.0 + alpha;
|
||||||
double a1 = -2.0*cos(w0);
|
double a1 = -2.0*cos(w0);
|
||||||
double a2 = 1.0 - alpha;
|
double a2 = 1.0 - alpha;
|
||||||
@@ -179,108 +195,112 @@ namespace IIR {
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
/** configure the filter as all-pass. freqFact between ]0;0.5[ */
|
// /** configure the filter as all-pass. freqFact between ]0;0.5[ */
|
||||||
void setAllPass( double freqFact, const float octaves ) {
|
// void setAllPass( double freqFact, const float octaves ) {
|
||||||
|
|
||||||
sanityCheck(freqFact);
|
// sanityCheck(freqFact);
|
||||||
|
|
||||||
double w0 = 2.0 * M_PI * 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 b0 = 1 - alpha;
|
// double b0 = 1 - alpha;
|
||||||
double b1 = -2*cos(w0);
|
// double b1 = -2*cos(w0);
|
||||||
double b2 = 1 + alpha;
|
// double b2 = 1 + alpha;
|
||||||
double a0 = 1 + alpha;
|
// double a0 = 1 + alpha;
|
||||||
double a1 = -2*cos(w0);
|
// double a1 = -2*cos(w0);
|
||||||
double a2 = 1 - alpha;
|
// double a2 = 1 - alpha;
|
||||||
|
|
||||||
setValues(a0, a1, a2, b0, b1, b2);
|
// setValues(a0, a1, a2, b0, b1, b2);
|
||||||
|
|
||||||
}
|
// }
|
||||||
|
|
||||||
/** configure the filter as all-pass */
|
// /** configure the filter as all-pass */
|
||||||
void setAllPass( const float freq, const float octaves, const float sRate ) {
|
// void setAllPass( const float freq, const float octaves, const float sRate ) {
|
||||||
double freqFact = double(freq) / double(sRate);
|
// double freqFact = double(freq) / double(sRate);
|
||||||
setAllPass(freqFact, octaves);
|
// 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 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);
|
// /** configure as notch filter. freqFact between ]0;0.5[ */
|
||||||
setHighShelf(freqFact, octaves, gain);
|
// //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:
|
protected:
|
||||||
|
|||||||
@@ -21,61 +21,6 @@
|
|||||||
*/
|
*/
|
||||||
class PoseDetection {
|
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 */
|
/** live-pose-estimation using moving average of the smartphone's accelerometer */
|
||||||
struct EstMovingAverage {
|
struct EstMovingAverage {
|
||||||
@@ -127,52 +72,52 @@ class PoseDetection {
|
|||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
/** live-pose-estimation using moving median of the smartphone's accelerometer */
|
// /** live-pose-estimation using moving median of the smartphone's accelerometer */
|
||||||
struct EstMovingMedian {
|
// struct EstMovingMedian {
|
||||||
|
|
||||||
// median the accelerometer
|
// // median the accelerometer
|
||||||
MovingMedianTS<float> medianX;
|
// MovingMedianTS<float> medianX;
|
||||||
MovingMedianTS<float> medianY;
|
// MovingMedianTS<float> medianY;
|
||||||
MovingMedianTS<float> medianZ;
|
// MovingMedianTS<float> medianZ;
|
||||||
|
|
||||||
EstMovingMedian(const Timestamp window) :
|
// EstMovingMedian(const Timestamp window) :
|
||||||
medianX(window), medianY(window), medianZ(window) {
|
// medianX(window), medianY(window), medianZ(window) {
|
||||||
|
|
||||||
// start approximately
|
// // start approximately
|
||||||
addAcc(Timestamp(), AccelerometerData(0,0,9.81));
|
// addAcc(Timestamp(), AccelerometerData(0,0,9.81));
|
||||||
|
|
||||||
}
|
// }
|
||||||
|
|
||||||
/** add the given accelerometer reading */
|
// /** add the given accelerometer reading */
|
||||||
void addAcc(const Timestamp ts, const AccelerometerData& acc) {
|
// void addAcc(const Timestamp ts, const AccelerometerData& acc) {
|
||||||
medianX.add(ts, acc.x);
|
// medianX.add(ts, acc.x);
|
||||||
medianY.add(ts, acc.y);
|
// medianY.add(ts, acc.y);
|
||||||
medianZ.add(ts, acc.z);
|
// medianZ.add(ts, acc.z);
|
||||||
}
|
// }
|
||||||
|
|
||||||
AccelerometerData getBase() const {
|
// AccelerometerData getBase() const {
|
||||||
return AccelerometerData(medianX.get(), medianY.get(), medianZ.get());
|
// return AccelerometerData(medianX.get(), medianY.get(), medianZ.get());
|
||||||
}
|
// }
|
||||||
|
|
||||||
/** get the current rotation matrix estimation */
|
// /** get the current rotation matrix estimation */
|
||||||
//Eigen::Matrix3f get() const {
|
// //Eigen::Matrix3f get() const {
|
||||||
Matrix3 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)
|
// // rotate average-accelerometer into (0,0,1)
|
||||||
const Vector3 zAxis(0,0,1);
|
// const Vector3 zAxis(0,0,1);
|
||||||
const Matrix3 rotMat = getRotationMatrix(base.normalized(), zAxis);
|
// const Matrix3 rotMat = getRotationMatrix(base.normalized(), zAxis);
|
||||||
|
|
||||||
// just a small sanity check. after applying to rotation the acc-average should become (0,0,1)
|
// // just a small sanity check. after applying to rotation the acc-average should become (0,0,1)
|
||||||
const Vector3 aligned = (rotMat * base).normalized();
|
// const Vector3 aligned = (rotMat * base).normalized();
|
||||||
Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high");
|
// 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 */
|
/** get the smartphone's rotation matrix */
|
||||||
const Matrix3& getMatrix() const {
|
const Matrix3& getMatrix() const {
|
||||||
return orientation.rotationMatrix;
|
return orientation.rotationMatrix;
|
||||||
@@ -236,22 +176,6 @@ public:
|
|||||||
|
|
||||||
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" */
|
/** get a matrix that rotates the vector "from" into the vector "to" */
|
||||||
static Matrix3 getRotationMatrix(const Vector3& from, const Vector3 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<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();
|
|
||||||
|
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -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<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
|
// add coordinate system
|
||||||
const Vector3 vx = rotation * Vector3(2,0,0);
|
const Vector3 vx = rotation * Vector3(2,0,0);
|
||||||
const Vector3 vy = rotation * Vector3(0,3,0);
|
const Vector3 vy = rotation * Vector3(0,3,0);
|
||||||
|
|||||||
Reference in New Issue
Block a user