Change ewma2 filter to compare filtered delta vs learned noise to

dynamically adjust filtering for each channel.

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 <algorithm>

 //#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<double>(std::max<double>(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<double>(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<double>(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;