From 48a8485317a90687e51419fe32c512cd1ebb2943 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Sun, 26 Oct 2014 09:09:46 +0100 Subject: rework accela, less nonsensical user-facing options --- .../ftnoir_accela_filtercontrols.ui | 152 ++++++++++----------- ftnoir_filter_accela/ftnoir_filter_accela.cpp | 49 ++++--- ftnoir_filter_accela/ftnoir_filter_accela.h | 32 ++--- .../ftnoir_filter_accela_dialog.cpp | 8 +- ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp | 2 +- opentrack/tracker.cpp | 4 +- 6 files changed, 109 insertions(+), 138 deletions(-) diff --git a/ftnoir_filter_accela/ftnoir_accela_filtercontrols.ui b/ftnoir_filter_accela/ftnoir_accela_filtercontrols.ui index 3c7f52ab..310b08cc 100644 --- a/ftnoir_filter_accela/ftnoir_accela_filtercontrols.ui +++ b/ftnoir_filter_accela/ftnoir_accela_filtercontrols.ui @@ -9,8 +9,8 @@ 0 0 - 277 - 447 + 288 + 432 @@ -39,36 +39,14 @@ - Rotation + Fast rotation speed Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - QAbstractSpinBox::CorrectToPreviousValue - - - 4 - - - 0.100000000000000 - - - 65535.000000000000000 - - - 1.000000000000000 - - - - + @@ -81,51 +59,6 @@ - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - 4 - - - 0.100000000000000 - - - 65535.000000000000000 - - - 1.000000000000000 - - - - - - - - 0 - 0 - - - - Fast alpha - - - - - - - 100 - - - 8 - - - Qt::Horizontal - - - @@ -200,8 +133,52 @@ background:none; - - + + + + 1 + + + 100 + + + 5 + + + Qt::Horizontal + + + QSlider::TicksAbove + + + 25 + + + + + + + 1 + + + 100 + + + 5 + + + Qt::Horizontal + + + QSlider::TicksAbove + + + 25 + + + + + 0 @@ -209,23 +186,32 @@ background:none; - Exponent + Slow rotation speed + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop - - - - 3 - + + - 0.050000000000000 + 1 - 100.000000000000000 + 100 - - 0.050000000000000 + + 5 + + + Qt::Horizontal + + + QSlider::TicksAbove + + + 25 diff --git a/ftnoir_filter_accela/ftnoir_filter_accela.cpp b/ftnoir_filter_accela/ftnoir_filter_accela.cpp index 8a98e528..370d19db 100644 --- a/ftnoir_filter_accela/ftnoir_filter_accela.cpp +++ b/ftnoir_filter_accela/ftnoir_filter_accela.cpp @@ -12,24 +12,24 @@ #include "opentrack/plugin-api.hpp" using namespace std; -FTNoIR_Filter::FTNoIR_Filter() : first_run(true), fast_state { 0,0,0, 0,0,0 } +FTNoIR_Filter::FTNoIR_Filter() : first_run(true), fast_state { 0,0,0 } { } -static inline double parabola(const double a, const double x, const double dz, const double expt) +static inline double dz(double x, double dz) { - const double sign = x > 0 ? 1 : -1; - const double a1 = 1./a; - return a1 * pow(std::max(0., fabs(x) - dz), expt) * sign; + return std::max(0., fabs(x) - dz) * (x < 0. ? -1. : 1.); } 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++) { - fast_state[i] = 0; output[i] = input[i]; for (int j = 0; j < 3; j++) last_output[i] = input[i]; @@ -38,46 +38,45 @@ void FTNoIR_Filter::filter(const double* input, double *output) return; } - const double fast_c = s.fast_alpha / 100.; const double rot_dz = s.rot_deadzone; const double trans_dz = s.trans_deadzone; - const double rot_a = s.rotation_alpha; - const double trans_a = s.translation_alpha; - const double expt = s.expt; + + const int s_rot_plus = s.rot_plus, s_rot_minus = s.rot_minus; + + 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.; static constexpr double fast_alpha = Hz/(Hz + fast_alpha_seconds); - for (int i=0;i<6;i++) + for (int i = 0; i < 6; i++) { const double vec = input[i] - last_output[i]; - const double a = i >= 3 ? rot_a : trans_a; - const double deadzone = i >= 3 ? rot_dz : trans_dz; - - double datum; + double datum = Hz * 16; if (i >= 3) { - const double cur_fast = std::abs(vec) * fast_alpha + fast_state[i]*(1. - fast_alpha); - fast_state[i] = cur_fast; - const double how_fast = std::max(0., fast_c * (cur_fast - max_slow_delta)); - datum = parabola(a, vec * (1.-damping + (1.+damping)*how_fast), deadzone, s.expt); + 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 = parabola(a, vec, deadzone, expt); + datum *= dz(vec, trans_dz) * a_trans; const double result = last_output[i] + datum; const bool negp = vec < 0.; const bool done = negp ? result <= input[i] : result >= input[i]; - last_output[i] = last_output[i]; - last_output[i] = last_output[i]; const double ret = done ? input[i] : result; last_output[i] = output[i] = ret; } - state.y = output[Yaw] - input[Yaw]; - state.p = output[Pitch] - input[Pitch]; - state.r = output[Roll] - input[Roll]; + 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 a7821516..afc85d47 100644 --- a/ftnoir_filter_accela/ftnoir_filter_accela.h +++ b/ftnoir_filter_accela/ftnoir_filter_accela.h @@ -5,30 +5,20 @@ #include #include -#define ACCELA_SMOOTHING_ROTATION 60.0 -#define ACCELA_SMOOTHING_TRANSLATION 40.0 -#define ACCELA_fast_ALPHA 100.0 -#define ACCELA_THIRD_ORDER_ALPHA 180.0 - #include "opentrack/options.hpp" using namespace options; struct settings { pbundle b; - value rotation_alpha, - translation_alpha, - rot_deadzone, - trans_deadzone, - expt; - value fast_alpha; + value rot_deadzone, trans_deadzone; + value rot_plus, rot_minus, trans_smoothing; settings() : b(bundle("Accela")), - rotation_alpha(b, "rotation-alpha", ACCELA_SMOOTHING_ROTATION), - translation_alpha(b, "translation-alpha", ACCELA_SMOOTHING_TRANSLATION), rot_deadzone(b, "rotation-deadband", 0), trans_deadzone(b, "translation-deadband", 0), - expt(b, "exponent", 2), - fast_alpha(b, "fast-alpha", 0) + rot_plus(b, "rotation-increase", 30), + rot_minus(b, "rotation-decrease", 25), + trans_smoothing(b, "translation-smoothing", 0) {} }; @@ -44,20 +34,16 @@ public: FTNoIR_Filter(); void filter(const double* target_camera_position, double *new_camera_position); state_display state; -private: - // hardcoded distance between filter() calls static constexpr double Hz = 3./1000; - // moving average history - static constexpr double fast_alpha_seconds = 0.6; +private: + // moving average history amount + static constexpr double fast_alpha_seconds = 0.7; // max degrees considered "slow" after moving average static constexpr double max_slow_delta = 0.9; - // if set to zero, never decreases response above, can also slow - // down due to fast_alpha - static constexpr double damping = 0.; settings s; bool first_run; double last_output[6]; - double fast_state[6]; + double fast_state[3]; }; class FilterControls: public IFilterDialog diff --git a/ftnoir_filter_accela/ftnoir_filter_accela_dialog.cpp b/ftnoir_filter_accela/ftnoir_filter_accela_dialog.cpp index 0f21cbcb..cbfe650f 100644 --- a/ftnoir_filter_accela/ftnoir_filter_accela_dialog.cpp +++ b/ftnoir_filter_accela/ftnoir_filter_accela_dialog.cpp @@ -12,12 +12,12 @@ FilterControls::FilterControls() : connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK())); connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel())); - tie_setting(s.rotation_alpha, ui.rotation_alpha); - tie_setting(s.translation_alpha, ui.translation_alpha); - tie_setting(s.fast_alpha, ui.ui_fast_alpha); tie_setting(s.rot_deadzone, ui.rot_deadzone); tie_setting(s.trans_deadzone, ui.trans_deadzone); - tie_setting(s.expt, ui.expt); + + 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())); diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp index 67dd8433..f7e1da5b 100644 --- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp +++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp @@ -219,7 +219,7 @@ void Tracker::run() std::vector< aruco::Marker > markers; - const double size_min = 0.07; + const double size_min = 0.02; const double size_max = 0.4; bool roi_valid = false; diff --git a/opentrack/tracker.cpp b/opentrack/tracker.cpp index ad1938f9..24482bfc 100644 --- a/opentrack/tracker.cpp +++ b/opentrack/tracker.cpp @@ -92,12 +92,12 @@ static cv::Matx33d euler_to_rmat(const double* input) void Tracker::t_compensate(const cv::Matx33d& rmat, const double* xyz, double* output, bool rz) { - const double xyz_[3] = { xyz[2], xyz[0], -xyz[1] }; + const double xyz_[3] = { xyz[2], -xyz[0], -xyz[1] }; cv::Matx31d tvec(xyz_); const cv::Matx31d ret = rmat * tvec; if (!rz) output[2] = ret(0, 0); - output[0] = ret(1, 0); + output[0] = -ret(1, 0); output[1] = -ret(2, 0); } -- cgit v1.2.3