From 9547e0aea4918da744f58d0b908a952f55a96df0 Mon Sep 17 00:00:00 2001 From: Donovan Baarda Date: Sun, 11 Aug 2013 13:49:04 +1000 Subject: Change ewma2 filter to compare filtered delta vs learned noise to dynamically adjust filtering for each channel. --- ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp | 63 +++++++++++++++-------------- ftnoir_filter_ewma2/ftnoir_filter_ewma2.h | 6 ++- 2 files changed, 37 insertions(+), 32 deletions(-) diff --git a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp index b5250593..8e9196ba 100644 --- a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp +++ b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp @@ -30,16 +30,26 @@ #include //#define LOG_OUTPUT -////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////// // // EWMA Filter: Exponentially Weighted Moving Average filter with dynamic smoothing parameter // -////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// This filter tries to adjust the amount of filtering to minimize lag when +// moving, and minimize noise when still. It uses the delta filtered over the +// last 3 frames (0.1secs) compared to the delta's average noise variance over +// the last 3600 frames (~2mins) to try and detect movement vs noise. As the +// delta increases from 0->3 stdevs of the noise, the filtering scales down +// from maxSmooth->minSmooth at a rate controlled by the powCurve setting. +// +/////////////////////////////////////////////////////////////////////////////// FTNoIR_Filter::FTNoIR_Filter() { first_run = true; - alpha_smoothing = 0.02f; // this is a constant for now, might be a parameter later + // Deltas are smoothed over the last 3 frames (0.1sec at 30fps). + delta_smoothing = 1.0 / 3.0; + // Noise is smoothed over the last 3600 frames (~2mins at 30fps). + noise_smoothing = 1.0 / 3600.0; loadSettings(); // Load the Settings } @@ -63,8 +73,10 @@ void FTNoIR_Filter::loadSettings() { // The EWMA2-filter-settings are in the Tracking group: this is because they used to be on the Main Form of FaceTrackNoIR // iniFile.beginGroup ( "Tracking" ); - kMinSmoothing = iniFile.value ( "minSmooth", 15 ).toInt(); - kMaxSmoothing = iniFile.value ( "maxSmooth", 50 ).toInt(); + // TODO(abo): change UI to have range 1-120 frames. + kMinSmoothing = iniFile.value ( "minSmooth", 2 ).toInt(); + kMaxSmoothing = iniFile.value ( "maxSmooth", 15 ).toInt(); + // TODO(abo): change powCurve to a float with default=1.0. kSmoothingScaleCurve = iniFile.value ( "powCurve", 10 ).toInt(); iniFile.endGroup (); } @@ -74,43 +86,35 @@ void FTNoIR_Filter::FilterHeadPoseData(double *current_camera_position, double *new_camera_position, double *last_post_filter) { - double delta; - double new_alpha; - double scale[]={0.025f,0.025f,0.025f,6.0f,6.0f,6.0f}; + double new_delta, new_noise, norm_noise; + double alpha; //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]; - alpha[i] = 0.0f; + delta[i] = 0.0f; + noise[i] = 0.0f; } first_run=false; return; } - for (int i=0;i<6;i++) { - // Calculate the delta. - delta=target_camera_position[i]-current_camera_position[i]; - // Normalise the delta. - delta=std::min(std::max(fabs(delta)/scale[i],0.0),1.0); - // Calculate the new alpha from the normalized delta. - new_alpha=1.0/(kMinSmoothing+((1.0-pow(delta,kSmoothingScaleCurve))*(kMaxSmoothing-kMinSmoothing))); - // Update the smoothed alpha. - alpha[i]=(alpha_smoothing*new_alpha)+((1.0f-alpha_smoothing)*alpha[i]); - } - - // Use the same (largest) smoothed alpha for each channel - //NB: larger alpha = *less* lag (opposite to what you'd expect) - float largest_alpha=0.0f; - for (int i=0;i<6;i++) { - largest_alpha=std::max(largest_alpha, alpha[i]); - } - // Calculate the new camera position. for (int i=0;i<6;i++) { - new_camera_position[i]=(largest_alpha*target_camera_position[i])+((1.0f-largest_alpha)*current_camera_position[i]); - //new_camera_position[i]=(alpha[i]*target_camera_position[i])+((1.0f-alpha[i])*current_camera_position[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/(kMinSmoothing+(1.0-pow(norm_noise,kSmoothingScaleCurve/20.0))*(kMaxSmoothing-kMinSmoothing)); + new_camera_position[i] = alpha*target_camera_position[i] + (1.0-alpha)*current_camera_position[i]; } #ifdef LOG_OUTPUT @@ -136,7 +140,6 @@ void FTNoIR_Filter::FilterHeadPoseData(double *current_camera_position, << "\t" << new_camera_position[3] << "\t" << new_camera_position[4] << "\t" << new_camera_position[5] << '\n'; - out << "largest_alpha:\t" << largest_alpha << '\n'; } #endif diff --git a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h index a5f3ef24..72dc6f0a 100644 --- a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h +++ b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h @@ -53,8 +53,10 @@ private: double newHeadPose; // Structure with new headpose bool first_run; - double alpha_smoothing; - double alpha[6]; + double delta_smoothing; + double noise_smoothing; + double delta[6]; + double noise[6]; double kMinSmoothing; double kMaxSmoothing; -- cgit v1.2.3