diff options
Diffstat (limited to 'filter-kalman/kalman.cpp')
| -rw-r--r-- | filter-kalman/kalman.cpp | 129 |
1 files changed, 129 insertions, 0 deletions
diff --git a/filter-kalman/kalman.cpp b/filter-kalman/kalman.cpp new file mode 100644 index 00000000..39a08703 --- /dev/null +++ b/filter-kalman/kalman.cpp @@ -0,0 +1,129 @@ +/* Copyright (c) 2013 Stanisław Halik <sthalik@misaki.pl> + * + * 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_kalman.h" +#include "opentrack/plugin-api.hpp" +#include <QDebug> +#include <cmath> + +constexpr double settings::mult_noise_stddev; + +FTNoIR_Filter::FTNoIR_Filter() { + reset(); + prev_slider_pos = s.noise_stddev_slider; +} + +// the following was written by Donovan Baarda <abo@minkirri.apana.org.au> +// https://sourceforge.net/p/facetracknoir/discussion/1150909/thread/418615e1/?limit=25#af75/084b +void FTNoIR_Filter::reset() { + // Setup kalman with state (x) is the 6 tracker outputs then + // their 6 corresponding velocities, and the measurement (z) is + // the 6 tracker outputs. + kalman.init(12, 6, 0, CV_64F); + // Initialize the transitionMatrix and processNoiseCov for + // dt=0.1. This needs to be updated each frame for the real dt + // value, but this hows you what they should look like. See + // http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical + double dt = 0.1; + kalman.transitionMatrix = (cv::Mat_<double>(12, 12) << + 1, 0, 0, 0, 0, 0, dt, 0, 0, 0, 0, 0, + 0, 1, 0, 0, 0, 0, 0, dt, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0, 0, 0, dt, 0, 0, 0, + 0, 0, 0, 1, 0, 0, 0, 0, 0, dt, 0, 0, + 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, dt, 0, + 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, dt, + 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1); + double accel_variance = accel_stddev * accel_stddev; + double a = dt * dt * accel_variance; // dt^2 * accel_variance. + double b = 0.5 * a * dt; // (dt^3)/2 * accel_variance. + double c = 0.5 * b * dt; // (dt^4)/4 * accel_variance. + kalman.processNoiseCov = (cv::Mat_<double>(12, 12) << + c, 0, 0, 0, 0, 0, b, 0, 0, 0, 0, 0, + 0, c, 0, 0, 0, 0, 0, b, 0, 0, 0, 0, + 0, 0, c, 0, 0, 0, 0, 0, b, 0, 0, 0, + 0, 0, 0, c, 0, 0, 0, 0, 0, b, 0, 0, + 0, 0, 0, 0, c, 0, 0, 0, 0, 0, b, 0, + 0, 0, 0, 0, 0, c, 0, 0, 0, 0, 0, b, + b, 0, 0, 0, 0, 0, a, 0, 0, 0, 0, 0, + 0, b, 0, 0, 0, 0, 0, a, 0, 0, 0, 0, + 0, 0, b, 0, 0, 0, 0, 0, a, 0, 0, 0, + 0, 0, 0, b, 0, 0, 0, 0, 0, a, 0, 0, + 0, 0, 0, 0, b, 0, 0, 0, 0, 0, a, 0, + 0, 0, 0, 0, 0, b, 0, 0, 0, 0, 0, a); + cv::setIdentity(kalman.measurementMatrix); + const double noise_stddev = (1+s.noise_stddev_slider) * s.mult_noise_stddev; + const double noise_variance = noise_stddev * noise_stddev; + cv::setIdentity(kalman.measurementNoiseCov, cv::Scalar::all(noise_variance)); + cv::setIdentity(kalman.errorCovPost, cv::Scalar::all(accel_variance * 1e4)); + for (int i = 0; i < 6; i++) { + last_input[i] = 0; + } + timer.invalidate(); +} + +void FTNoIR_Filter::filter(const double* input, double *output) +{ + if (prev_slider_pos != s.noise_stddev_slider) + { + reset(); + prev_slider_pos = s.noise_stddev_slider; + } + // Start the timer if it's not running. + if (!timer.isValid()) + timer.start(); + // Get the time in seconds since last run and restart the timer. + auto dt = timer.restart() / 1000.0f; + // Note this is a terrible way to detect when there is a new + // frame of tracker input, but it is the best we have. + bool new_input = false; + for (int i = 0; i < 6 && !new_input; i++) + new_input = (input[i] != last_input[i]); + // Update the transitionMatrix and processNoiseCov for dt. + double accel_variance = accel_stddev * accel_stddev; + double a = dt * dt * accel_variance; // dt^2 * accel_variance. + double b = 0.5 * a * dt; // (dt^3)/2 * accel_variance. + double c = 0.5 * b * dt; // (dt^4)/4 * accel_variance. + for (int i = 0; i < 6; i++) { + kalman.transitionMatrix.at<double>(i,i+6) = dt; + kalman.processNoiseCov.at<double>(i,i) = c; + kalman.processNoiseCov.at<double>(i+6,i+6) = a; + kalman.processNoiseCov.at<double>(i,i+6) = b; + kalman.processNoiseCov.at<double>(i+6,i) = b; + } + // Get the updated predicted position. + cv::Mat next_output = kalman.predict(); + // If we have new tracker input, get the corrected position. + if (new_input) { + cv::Mat measurement(6, 1, CV_64F); + for (int i = 0; i < 6; i++) { + measurement.at<double>(i) = input[i]; + // Save last_input for detecting new tracker input. + last_input[i] = input[i]; + } + next_output = kalman.correct(measurement); + } + // Set output to the next_output. + for (int i = 0; i < 6; i++) { + output[i] = next_output.at<double>(i); + } +} + +void FilterControls::doOK() { + s.b->save(); + close(); +} + +void FilterControls::doCancel() { + s.b->reload(); + close(); +} + +OPENTRACK_DECLARE_FILTER(FTNoIR_Filter, FilterControls, FTNoIR_FilterDll) |