From ec207728c97e5f483ce1f9b0ff7449b8c6092bb5 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Thu, 27 Nov 2014 19:18:14 +0100 Subject: accela: rework to remove deadzone need Tested-by: @KyokushinPL filter gain adjusted by a slider. frames smoothed by ewma for sudden noise spike reduction. likely need further work for arbitrary magic constants embedded in tracker gain. --- .../ftnoir_accela_filtercontrols.ui | 280 +++------------------ ftnoir_filter_accela/ftnoir_filter_accela.cpp | 93 +++---- ftnoir_filter_accela/ftnoir_filter_accela.h | 30 +-- .../ftnoir_filter_accela_dialog.cpp | 26 +- 4 files changed, 99 insertions(+), 330 deletions(-) diff --git a/ftnoir_filter_accela/ftnoir_accela_filtercontrols.ui b/ftnoir_filter_accela/ftnoir_accela_filtercontrols.ui index 310b08cc..4c836ac9 100644 --- a/ftnoir_filter_accela/ftnoir_accela_filtercontrols.ui +++ b/ftnoir_filter_accela/ftnoir_accela_filtercontrols.ui @@ -9,8 +9,8 @@ 0 0 - 288 - 432 + 424 + 268 @@ -30,36 +30,7 @@ QFrame::Raised - - - - - 0 - 0 - - - - Fast rotation speed - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop - - - - - - - - 0 - 0 - - - - Translation - - - - + @@ -72,33 +43,40 @@ background:none; - Rotation deadband + Rotation smoothing - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - 3 - - - 0.000000000000000 - - - 100.000000000000000 - - - 0.050000000000000 - - - 0.000000000000000 - + + + + None + + + + + Low + + + + + Normal + + + + + High + + + + + Extreme + + - + @@ -107,37 +85,12 @@ background:none; - Translation deadband - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - 3 - - - 0.000000000000000 - - - 100.000000000000000 - - - 0.050000000000000 - - - 0.000000000000000 + Translation smoothing - - - 1 - + 100 @@ -155,49 +108,8 @@ background:none; - - - - 1 - - - 100 - - - 5 - - - Qt::Horizontal - - - QSlider::TicksAbove - - - 25 - - - - - - - - 0 - 0 - - - - Slow rotation speed - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop - - - - - - 1 - + 100 @@ -215,126 +127,10 @@ background:none; - - - - - - - Debug information - - - - - - - 0 - 0 - - - - Yaw - - - - - - - - 0 - 0 - - - - Pitch - - - - - - - - 0 - 0 - - + + - Roll - - - - - - - - 0 - 0 - - - - true - - - QAbstractSpinBox::PlusMinus - - - 8 - - - 99.000000000000000 - - - 0.000000000000000 - - - - - - - - 0 - 0 - - - - true - - - QAbstractSpinBox::PlusMinus - - - 8 - - - 99.000000000000000 - - - 0.000000000000000 - - - - - - - - 0 - 0 - - - - true - - - QAbstractSpinBox::PlusMinus - - - 8 - - - 99.000000000000000 - - - 0.000000000000000 + Spiking noise reduction @@ -358,7 +154,7 @@ background:none; QFrame::NoFrame - <html><head/><body><p align="justify"><br/></p><p align="justify"><span style=" font-size:10pt;">Accela by </span><a href="https://github.com/sthalik"><span style=" font-size:10pt; text-decoration: underline; color:#0057ae;">Stanisław Halik</span></a><span style=" font-size:10pt;"><br/>Thanks to </span><a href="https://github.com/dbaarda"><span style=" font-size:10pt; text-decoration: underline; color:#0057ae;">Donovan Baarda</span></a></p><p align="right"><span style=" font-size:10pt;">2012-2014</span></p></body></html> + <html><head/><body><p align="justify"><br/><span style=" font-size:10pt;">Accela by </span><a href="https://github.com/sthalik"><span style=" font-size:10pt; text-decoration: underline; color:#0057ae;">Stanisław Halik</span></a><span style=" font-size:10pt;"><br/>Thanks to </span><a href="https://github.com/dbaarda"><span style=" font-size:10pt; text-decoration: underline; color:#0057ae;">Donovan Baarda</span></a></p><p align="right"><span style=" font-size:10pt;">2012-2014</span></p><p align="right"><br/></p></body></html> Qt::RichText diff --git a/ftnoir_filter_accela/ftnoir_filter_accela.cpp b/ftnoir_filter_accela/ftnoir_filter_accela.cpp index 42478963..d5cd4080 100644 --- a/ftnoir_filter_accela/ftnoir_filter_accela.cpp +++ b/ftnoir_filter_accela/ftnoir_filter_accela.cpp @@ -12,74 +12,83 @@ #include "opentrack/plugin-api.hpp" using namespace std; -FTNoIR_Filter::FTNoIR_Filter() : first_run(true), fast_state { 0,0,0 } +FTNoIR_Filter::FTNoIR_Filter() : first_run(true) { } -static inline double dz(double x, double dz) +static double f(double vec, double thres) { - return std::max(0., fabs(x) - dz) * (x < 0. ? -1. : 1.); + if (vec > thres*4) + return (vec - thres*4) * 600 + thres*4; + if (vec > thres) + return (vec - thres) * 150 + thres; + return vec; } void FTNoIR_Filter::filter(const double* input, double *output) { if (first_run) { - for (int i = 0; i < 3; i++) - fast_state[i] = 0; - for (int i = 0; i < 6; i++) { output[i] = input[i]; - for (int j = 0; j < 3; j++) - last_output[i] = input[i]; + last_output[i] = input[i]; + smoothed_input[i] = input[i]; } first_run = false; - timer.start(); + t.start(); return; } - const double rot_dz = s.rot_deadzone; - const double trans_dz = s.trans_deadzone; - - const int s_rot_plus = s.rot_plus, s_rot_minus = s.rot_minus; + const double rot_t = 10. * s.rot_threshold / 100.; + const double trans_t = 10. * s.trans_threshold / 100.; - const double a_rot_plus = s_rot_plus/100.; - const double a_rot_minus = s_rot_minus/100. * a_rot_plus; - const double a_trans = s.trans_smoothing/100.; + const double dt = t.elapsed() * 1e-9; + t.start(); - const double Hz = timer.elapsed() * 1e-9; - timer.start(); - double fast_alpha = Hz/(Hz + fast_alpha_seconds); + double RC; + switch (s.ewma) + { + default: + case 0: // none + RC = 0; + break; + case 1: // low + RC = 0.07; + break; + case 2: // normal + RC = 0.10; + break; + case 3: // high + RC = 0.14; + break; + case 4: // extreme + RC = 0.16; + break; + } + for (int i = 0; i < 6; i++) { - const double vec = input[i] - last_output[i]; - double datum = Hz * 16; - - if (i >= 3) - { - int k = i - 3; - const double vec_ = dz(vec, rot_dz); - const double cur_fast = fabs(vec_) * fast_alpha + fast_state[k]*(1. - fast_alpha); - fast_state[k] = cur_fast; - const double c = cur_fast > max_slow_delta ? a_rot_plus : a_rot_minus; - datum *= vec_ * c; - } - else - datum *= dz(vec, trans_dz) * a_trans; - - const double result = last_output[i] + datum; + const double alpha = dt/(dt+RC); + + smoothed_input[i] = smoothed_input[i] * (1.-alpha) + input[i] * alpha; + + const double in = smoothed_input[i]; + + const double vec = in - last_output[i]; + const double vec_ = fabs(vec); + const double t = i >= 3 ? rot_t : trans_t; + const double val = f(vec_, t); + const double result = last_output[i] + (vec < 0 ? -1 : 1) * dt * val; const bool negp = vec < 0.; - const bool done = negp ? result <= input[i] : result >= input[i]; - - const double ret = done ? input[i] : result; + const bool done = negp + ? result <= in + : result >= in; + const double ret = done ? in : result; + last_output[i] = output[i] = ret; } - - state.y = fast_state[0]; - state.p = fast_state[1]; - state.r = fast_state[2]; } extern "C" OPENTRACK_EXPORT IFilter* GetConstructor() diff --git a/ftnoir_filter_accela/ftnoir_filter_accela.h b/ftnoir_filter_accela/ftnoir_filter_accela.h index 9052f49b..3abd2cf0 100644 --- a/ftnoir_filter_accela/ftnoir_filter_accela.h +++ b/ftnoir_filter_accela/ftnoir_filter_accela.h @@ -11,40 +11,26 @@ using namespace options; struct settings { pbundle b; - value rot_deadzone, trans_deadzone; - value rot_plus, rot_minus, trans_smoothing; + value rot_threshold, trans_threshold, ewma; settings() : b(bundle("Accela")), - rot_deadzone(b, "rotation-deadband", 0), - trans_deadzone(b, "translation-deadband", 0), - rot_plus(b, "rotation-increase", 30), - rot_minus(b, "rotation-decrease", 25), - trans_smoothing(b, "translation-smoothing", 50) + rot_threshold(b, "rotation-threshold", 30), + trans_threshold(b, "translation-threshold", 50), + ewma(b, "ewma", 2) {} }; -struct state_display -{ - double y, p, r; - state_display() : y(0), p(0), r(0) {} -}; - class FTNoIR_Filter : public IFilter { public: FTNoIR_Filter(); - void filter(const double* target_camera_position, double *new_camera_position); - state_display state; + void filter(const double* input, double *output); private: - // moving average history amount in seconds - static constexpr double fast_alpha_seconds = 0.3; - // max degrees considered "slow" after moving average - static constexpr double max_slow_delta = 0.9; settings s; bool first_run; double last_output[6]; - double fast_state[3]; - Timer timer; + double smoothed_input[6]; + Timer t; }; class FilterControls: public IFilterDialog @@ -60,11 +46,9 @@ private: void save(); FTNoIR_Filter* accela_filter; settings s; - QTimer t; private slots: void doOK(); void doCancel(); - void timer_fired(); }; class FTNoIR_FilterDll : public Metadata diff --git a/ftnoir_filter_accela/ftnoir_filter_accela_dialog.cpp b/ftnoir_filter_accela/ftnoir_filter_accela_dialog.cpp index cbfe650f..4fbc482d 100644 --- a/ftnoir_filter_accela/ftnoir_filter_accela_dialog.cpp +++ b/ftnoir_filter_accela/ftnoir_filter_accela_dialog.cpp @@ -12,38 +12,18 @@ FilterControls::FilterControls() : connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK())); connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel())); - tie_setting(s.rot_deadzone, ui.rot_deadzone); - tie_setting(s.trans_deadzone, ui.trans_deadzone); - - tie_setting(s.rot_plus , ui.rot_plus); - tie_setting(s.rot_minus , ui.rot_minus); - tie_setting(s.trans_smoothing, ui.trans_smoothing); - - connect(&t, SIGNAL(timeout()), this, SLOT(timer_fired())); - - t.setInterval(250); -} - -void FilterControls::timer_fired() -{ - if (accela_filter) - { - state_display st = accela_filter->state; - ui.debug_y->setValue(st.y); - ui.debug_p->setValue(st.p); - ui.debug_r->setValue(st.r); - } + tie_setting(s.rot_threshold, ui.rotation_slider); + tie_setting(s.trans_threshold, ui.translation_slider); + tie_setting(s.ewma, ui.ewma); } void FilterControls::register_filter(IFilter* filter) { accela_filter = static_cast(filter); - t.start(); } void FilterControls::unregister_filter() { - t.stop(); accela_filter = nullptr; } -- cgit v1.2.3