From b82ddb17360d66ad7270ad58409f53a6e1900437 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Mon, 16 Jun 2014 03:00:27 +0200 Subject: Revert "update accela, ewma to use lerp class" This reverts commit 7428b028ffd95c229c57f8089b97e6bd0af34339. --- 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, 76 insertions(+), 50 deletions(-) diff --git a/ftnoir_filter_accela/ftnoir_filter_accela.cpp b/ftnoir_filter_accela/ftnoir_filter_accela.cpp index a82ff077..9fcf4b92 100644 --- a/ftnoir_filter_accela/ftnoir_filter_accela.cpp +++ b/ftnoir_filter_accela/ftnoir_filter_accela.cpp @@ -30,40 +30,57 @@ 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; - } + return; + } + + bool new_frame = false; - if (!l.idempotentp(target_camera_position)) + for (int i = 0; i < 6; i++) { - for (int i=0;i<6;i++) + if (target_camera_position[i] != last_input[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] = done ? target_camera_position[i] : result; + new_frame = true; + break; } } - l.write(last_output[0], new_camera_position); + 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; + } } 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 2187fd01..8cf7fe66 100644 --- a/ftnoir_filter_accela/ftnoir_filter_accela.h +++ b/ftnoir_filter_accela/ftnoir_filter_accela.h @@ -2,7 +2,6 @@ #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 @@ -45,11 +44,11 @@ public: void receiveSettings() { s.b->reload(); } + private: - lerp l; settings s; bool first_run; - double last_output[3][6]; + double last_input[6], 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 dcc1475c..c073bfa4 100644 --- a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp +++ b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp @@ -64,43 +64,54 @@ 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; } - if (!l.idempotentp(target_camera_position)) + bool new_frame = false; + + for (int i = 0; i < 6; i++) { - 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 (target_camera_position[i] != current_camera_position[i]) + { + new_frame = true; + break; } } - l.write(pos, new_camera_position); + 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; + } } 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 ea25a135..bde3e79c 100644 --- a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h +++ b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h @@ -32,7 +32,6 @@ #include #include #include "facetracknoir/options.h" -#include "facetracknoir/lerp.hpp" using namespace options; struct settings { @@ -62,8 +61,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