From b1b442b17a626974c8840dd6a3912e078f00883e Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Sat, 14 Jun 2014 12:48:28 +0200 Subject: update accela, ewma to use lerp class --- ftnoir_filter_accela/ftnoir_filter_accela.cpp | 63 ++++++++++----------------- ftnoir_filter_accela/ftnoir_filter_accela.h | 5 ++- ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp | 55 ++++++++++------------- ftnoir_filter_ewma2/ftnoir_filter_ewma2.h | 3 +- 4 files changed, 50 insertions(+), 76 deletions(-) diff --git a/ftnoir_filter_accela/ftnoir_filter_accela.cpp b/ftnoir_filter_accela/ftnoir_filter_accela.cpp index 9fcf4b92..a82ff077 100644 --- a/ftnoir_filter_accela/ftnoir_filter_accela.cpp +++ b/ftnoir_filter_accela/ftnoir_filter_accela.cpp @@ -30,57 +30,40 @@ void FTNoIR_Filter::FilterHeadPoseData(const double* target_camera_position, { for (int i = 0; i < 6; i++) { - new_camera_position[i] = target_camera_position[i]; - last_input[i] = target_camera_position[i]; for (int j = 0; j < 3; j++) last_output[j][i] = target_camera_position[i]; + l.write(target_camera_position, new_camera_position); } first_run = false; - return; - } - - bool new_frame = false; + return; + } - for (int i = 0; i < 6; i++) + if (!l.idempotentp(target_camera_position)) { - if (target_camera_position[i] != last_input[i]) + for (int i=0;i<6;i++) { - new_frame = true; - break; + const double vec = target_camera_position[i] - last_output[0][i]; + const double vec2 = target_camera_position[i] - last_output[1][i]; + const double vec3 = target_camera_position[i] - last_output[2][i]; + const int sign = vec < 0 ? -1 : 1; + const double a = i >= 3 ? s.rotation_alpha : s.translation_alpha; + const double a2 = a * s.second_order_alpha; + const double a3 = a * s.third_order_alpha; + const double deadzone = i >= 3 ? s.rot_deadzone : s.trans_deadzone; + const double velocity = + parabola(a, vec, deadzone, s.expt) + + parabola(a2, vec2, deadzone, s.expt) + + parabola(a3, vec3, deadzone, s.expt); + const double result = last_output[0][i] + velocity; + const bool done = sign > 0 ? result >= target_camera_position[i] : result <= target_camera_position[i]; + last_output[2][i] = last_output[1][i]; + last_output[1][i] = last_output[0][i]; + last_output[0][i] = done ? target_camera_position[i] : result; } } - if (!new_frame) - { - for (int i = 0; i < 6; i++) - new_camera_position[i] = last_output[0][i]; - return; - } - - for (int i = 0; i < 6; i++) - last_input[i] = target_camera_position[i]; - - for (int i=0;i<6;i++) - { - const double vec = target_camera_position[i] - last_output[0][i]; - const double vec2 = target_camera_position[i] - last_output[1][i]; - const double vec3 = target_camera_position[i] - last_output[2][i]; - const int sign = vec < 0 ? -1 : 1; - const double a = i >= 3 ? s.rotation_alpha : s.translation_alpha; - const double a2 = a * s.second_order_alpha; - const double a3 = a * s.third_order_alpha; - const double deadzone = i >= 3 ? s.rot_deadzone : s.trans_deadzone; - const double velocity = - parabola(a, vec, deadzone, s.expt) + - parabola(a2, vec2, deadzone, s.expt) + - parabola(a3, vec3, deadzone, s.expt); - const double result = last_output[0][i] + velocity; - const bool done = sign > 0 ? result >= target_camera_position[i] : result <= target_camera_position[i]; - last_output[2][i] = last_output[1][i]; - last_output[1][i] = last_output[0][i]; - last_output[0][i] = new_camera_position[i] = done ? target_camera_position[i] : result; - } + l.write(last_output[0], new_camera_position); } extern "C" FTNOIR_FILTER_BASE_EXPORT IFilter* CALLING_CONVENTION GetConstructor() diff --git a/ftnoir_filter_accela/ftnoir_filter_accela.h b/ftnoir_filter_accela/ftnoir_filter_accela.h index 8cf7fe66..2187fd01 100644 --- a/ftnoir_filter_accela/ftnoir_filter_accela.h +++ b/ftnoir_filter_accela/ftnoir_filter_accela.h @@ -2,6 +2,7 @@ #include "ftnoir_filter_base/ftnoir_filter_base.h" #include "ui_ftnoir_accela_filtercontrols.h" #include "facetracknoir/global-settings.h" +#include "facetracknoir/lerp.hpp" #include #define ACCELA_SMOOTHING_ROTATION 60.0 @@ -44,11 +45,11 @@ public: void receiveSettings() { s.b->reload(); } - private: + lerp l; settings s; bool first_run; - double last_input[6], last_output[3][6]; + double last_output[3][6]; }; class FilterControls: public QWidget, public IFilterDialog diff --git a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp index c073bfa4..dcc1475c 100644 --- a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp +++ b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp @@ -64,54 +64,43 @@ void FTNoIR_Filter::FilterHeadPoseData(const double *target_camera_position, { double new_delta, new_noise, norm_noise; double alpha; + double pos[6]; //On the first run, initialize to output=target and return. if (first_run==true) { for (int i=0;i<6;i++) { - new_camera_position[i] = target_camera_position[i]; - current_camera_position[i] = target_camera_position[i]; delta[i] = 0.0; noise[i] = 0.0; + l.write(target_camera_position, new_camera_position); } first_run=false; return; } - bool new_frame = false; - - for (int i = 0; i < 6; i++) + if (!l.idempotentp(target_camera_position)) { - if (target_camera_position[i] != current_camera_position[i]) - { - new_frame = true; - break; + double cur[6]; + l.get_state(cur); + // Calculate the new camera position. + for (int i=0;i<6;i++) { + // Calculate the current and smoothed delta. + new_delta = target_camera_position[i]-cur[i]; + delta[i] = delta_smoothing*new_delta + (1.0-delta_smoothing)*delta[i]; + // Calculate the current and smoothed noise variance. + new_noise = delta[i]*delta[i]; + noise[i] = noise_smoothing*new_noise + (1.0-noise_smoothing)*noise[i]; + // Normalise the noise between 0->1 for 0->9 variances (0->3 stddevs). + norm_noise = std::min(new_noise/(9.0*noise[i]), 1.0); + // Calculate the alpha from the normalized noise. + // TODO(abo): change kSmoothingScaleCurve to a float where 1.0 is sqrt(norm_noise). + alpha = 1.0/(s.kMinSmoothing+(1.0-pow(norm_noise,s.kSmoothingScaleCurve/20.0))*(s.kMaxSmoothing-s.kMinSmoothing)); + // Update the current camera position to the new position. + double value = alpha*target_camera_position[i] + (1.0-alpha)*cur[i]; + pos[i] = value; } } - if (!new_frame) - { - for (int i = 0; i < 6; i++) - new_camera_position[i] = current_camera_position[i]; - return; - } - - // Calculate the new camera position. - for (int i=0;i<6;i++) { - // Calculate the current and smoothed delta. - new_delta = target_camera_position[i]-current_camera_position[i]; - delta[i] = delta_smoothing*new_delta + (1.0-delta_smoothing)*delta[i]; - // Calculate the current and smoothed noise variance. - new_noise = delta[i]*delta[i]; - noise[i] = noise_smoothing*new_noise + (1.0-noise_smoothing)*noise[i]; - // Normalise the noise between 0->1 for 0->9 variances (0->3 stddevs). - norm_noise = std::min(new_noise/(9.0*noise[i]), 1.0); - // Calculate the alpha from the normalized noise. - // TODO(abo): change kSmoothingScaleCurve to a float where 1.0 is sqrt(norm_noise). - alpha = 1.0/(s.kMinSmoothing+(1.0-pow(norm_noise,s.kSmoothingScaleCurve/20.0))*(s.kMaxSmoothing-s.kMinSmoothing)); - // Update the current camera position to the new position. - double pos = alpha*target_camera_position[i] + (1.0-alpha)*current_camera_position[i]; - new_camera_position[i] = current_camera_position[i] = pos; - } + l.write(pos, new_camera_position); } extern "C" FTNOIR_FILTER_BASE_EXPORT IFilter* CALLING_CONVENTION GetConstructor() diff --git a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h index bde3e79c..ea25a135 100644 --- a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h +++ b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h @@ -32,6 +32,7 @@ #include #include #include "facetracknoir/options.h" +#include "facetracknoir/lerp.hpp" using namespace options; struct settings { @@ -61,8 +62,8 @@ private: double noise_smoothing; double delta[6]; double noise[6]; - double current_camera_position[6]; settings s; + lerp l; }; class FilterControls: public QWidget, public IFilterDialog -- cgit v1.2.3