summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp60
-rw-r--r--ftnoir_filter_ewma2/ftnoir_filter_ewma2.h20
2 files changed, 39 insertions, 41 deletions
diff --git a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp
index b25fc50b..cdf18263 100644
--- a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp
+++ b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp
@@ -22,17 +22,9 @@
// to minSmooth at a rate controlled by the powCurve setting.
-FTNoIR_Filter::FTNoIR_Filter() :
- first_run(true),
- // 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))
+FTNoIR_Filter::FTNoIR_Filter()
{
+ reset();
}
void FTNoIR_Filter::receiveSettings()
@@ -40,43 +32,47 @@ void FTNoIR_Filter::receiveSettings()
s.b->reload();
}
-void FTNoIR_Filter::filter(const double *target_camera_position,
- double *new_camera_position)
+void FTNoIR_Filter::reset()
{
- double new_delta, new_noise, norm_noise;
- double smoothing, RC, alpha;
+ timer.invalidate();
+}
- //On the first run, initialize filter states to target intput.
- if (first_run==true) {
+void FTNoIR_Filter::filter(const double *input, double *output)
+{
+ // Start the timer and initialise filter state if it's not running.
+ if (!timer.isValid()) {
+ timer.start();
for (int i=0;i<6;i++) {
- output[i] = target_camera_position[i];
- delta[i] = 0.0;
- noise[i] = 0.0;
+ last_output[i] = input[i];
+ last_delta[i] = 0.0;
+ last_noise[i] = 0.0;
}
- first_run=false;
}
-
+ // Get the time in seconds since last run and restart the timer.
+ auto dt = timer.restart() / 1000.0f;
+ // Calculate delta_alpha and noise_alpha from dt.
+ double delta_alpha = dt/(dt + delta_RC);
+ double noise_alpha = dt/(dt + noise_RC);
// Calculate the new camera position.
for (int i=0;i<6;i++) {
// Calculate the current and smoothed delta.
- new_delta = target_camera_position[i]-output[i];
- delta[i] = delta_alpha*new_delta + (1.0-delta_alpha)*delta[i];
+ double delta = input[i] - last_output[i];
+ last_delta[i] = delta_alpha*delta + (1.0-delta_alpha)*last_delta[i];
// Calculate the current and smoothed noise variance.
- new_noise = delta[i]*delta[i];
- noise[i] = noise_alpha*new_noise + (1.0-noise_alpha)*noise[i];
+ double noise = last_delta[i]*last_delta[i];
+ last_noise[i] = noise_alpha*noise + (1.0-noise_alpha)*last_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);
+ double norm_noise = std::min<double>(noise/(9.0*last_noise[i]), 1.0);
// 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).
- smoothing = 1.0 - pow(norm_noise, s.kSmoothingScaleCurve/20.0);
+ double 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);
+ double RC = 3.0*(s.kMinSmoothing + smoothing*(s.kMaxSmoothing - s.kMinSmoothing))/100.0;
+ // Calculate the dynamic alpha.
+ double alpha = dt/(dt + RC);
// Calculate the new output position.
- output[i] = alpha*target_camera_position[i] + (1.0-alpha)*output[i];
- new_camera_position[i] = output[i];
+ output[i] = last_output[i] = alpha*input[i] + (1.0-alpha)*last_output[i];
}
}
diff --git a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h
index 0388c5f0..73c0df99 100644
--- a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h
+++ b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h
@@ -2,6 +2,7 @@
#include "opentrack/plugin-api.hpp"
#include "ui_ftnoir_ewma_filtercontrols.h"
+#include <QElapsedTimer>
#include <QWidget>
#include <QMutex>
#include "opentrack/options.hpp"
@@ -24,17 +25,18 @@ class FTNoIR_Filter : public IFilter
{
public:
FTNoIR_Filter();
- void reset() { first_run=true; }
- void filter(const double *target_camera_position,
- double *new_camera_position);
+ void reset();
+ void filter(const double *input, double *output);
void receiveSettings();
private:
- bool first_run;
- double delta_alpha;
- double noise_alpha;
- double delta[6];
- double noise[6];
- double output[6];
+ // Deltas are smoothed over the last 1/60sec (16ms).
+ const double delta_RC = 0.016;
+ // Noise is smoothed over the last 60sec.
+ const double noise_RC = 60.0;
+ double last_delta[6];
+ double last_noise[6];
+ double last_output[6];
+ QElapsedTimer timer;
settings s;
};