summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp91
-rw-r--r--ftnoir_filter_ewma2/ftnoir_filter_ewma2.h8
2 files changed, 44 insertions, 55 deletions
diff --git a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp
index c7169faa..320b95ad 100644
--- a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp
+++ b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp
@@ -1,19 +1,9 @@
-/*** Written by Donovan Baarda
-*
-* This program is free software; you can redistribute it and/or modify it *
-* under the terms of the GNU General Public License as published by the *
-* Free Software Foundation; either version 3 of the License, or (at your *
-* option) any later version. *
-* *
-* This program is distributed in the hope that it will be useful, but *
-* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY *
-* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for *
-* more details. *
-* *
-* You should have received a copy of the GNU General Public License along *
-* with this program; if not, see <http://www.gnu.org/licenses/>. *
-* *
-********************************************************************************/
+/* Copyright (c) 2014 Donovan Baarda <abo@minkirri.apana.org.au>
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ */
#include "ftnoir_filter_ewma2.h"
#include <cmath>
#include <QDebug>
@@ -22,12 +12,26 @@
#include <algorithm>
#include <QMutexLocker>
+// Exponentially Weighted Moving Average (EWMA) Filter with dynamic smoothing.
+//
+// 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 1/60sec (16ms) compared to the delta's average noise variance over
+// the last 60sec to try and detect movement vs noise. As the delta increases
+// from 0 to 3 stdevs of the noise, the filtering scales down from maxSmooth
+// to minSmooth at a rate controlled by the powCurve setting.
+
+
FTNoIR_Filter::FTNoIR_Filter() :
first_run(true),
- // 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)
+ // Currently facetracknoir/tracker.cpp updates every dt=3ms. All
+ // filter alpha values are calculated as alpha=dt/(dt+RC) and
+ // need to be updated when tracker.cpp changes.
+ // TODO(abo): Change this to use a dynamic dt using a timer.
+ // Deltas are smoothed over the last 1/60sec (16ms).
+ delta_alpha(0.003/(0.003 + 0.016)),
+ // Noise is smoothed over the last 60sec.
+ noise_alpha(0.003/(0.003 + 60.0))
{
}
@@ -40,54 +44,39 @@ void FTNoIR_Filter::FilterHeadPoseData(const double *target_camera_position,
double *new_camera_position)
{
double new_delta, new_noise, norm_noise;
- double alpha;
+ double smoothing, RC, alpha;
- //On the first run, initialize to output=target and return.
+ //On the first run, initialize filter states to target intput.
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];
+ output[i] = target_camera_position[i];
delta[i] = 0.0;
noise[i] = 0.0;
}
first_run=false;
- return;
- }
-
- bool new_frame = false;
-
- for (int i = 0; i < 6; i++)
- {
- if (target_camera_position[i] != current_camera_position[i])
- {
- new_frame = true;
- break;
- }
- }
-
- 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];
+ new_delta = target_camera_position[i]-output[i];
+ delta[i] = delta_alpha*new_delta + (1.0-delta_alpha)*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];
+ noise[i] = noise_alpha*new_noise + (1.0-noise_alpha)*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.
+ // Calculate the smoothing 0.0->1.0 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;
+ smoothing = 1.0 - pow(norm_noise, s.kSmoothingScaleCurve/20.0);
+ // Currently min/max smoothing are ints 0->100. We want 0.0->3.0 seconds.
+ // TODO(abo): Change kMinSmoothing, kMaxSmoothing to floats 0.0->3.0 seconds RC.
+ RC = 3.0*(s.kMinSmoothing + smoothing*(s.kMaxSmoothing - s.kMinSmoothing))/100.0;
+ // TODO(abo): Change this to use a dynamic dt using a timer.
+ alpha = 0.003/(0.003 + RC);
+ // Calculate the new output position.
+ output[i] = alpha*target_camera_position[i] + (1.0-alpha)*output[i];
+ new_camera_position[i] = output[i];
}
}
diff --git a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h
index 8863348c..aacf4916 100644
--- a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h
+++ b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h
@@ -24,17 +24,17 @@ class FTNoIR_Filter : public IFilter
{
public:
FTNoIR_Filter();
- void reset() {}
+ void reset() { first_run=true; }
void FilterHeadPoseData(const double *target_camera_position,
double *new_camera_position);
void receiveSettings();
private:
bool first_run;
- double delta_smoothing;
- double noise_smoothing;
+ double delta_alpha;
+ double noise_alpha;
double delta[6];
double noise[6];
- double current_camera_position[6];
+ double output[6];
settings s;
};