summaryrefslogtreecommitdiffhomepage
path: root/filter-kalman
diff options
context:
space:
mode:
Diffstat (limited to 'filter-kalman')
-rw-r--r--filter-kalman/CMakeLists.txt4
-rw-r--r--filter-kalman/ftnoir_filter_kalman.h73
-rw-r--r--filter-kalman/ftnoir_kalman_filtercontrols.ui116
-rw-r--r--filter-kalman/kalman.cpp129
4 files changed, 322 insertions, 0 deletions
diff --git a/filter-kalman/CMakeLists.txt b/filter-kalman/CMakeLists.txt
new file mode 100644
index 00000000..666a35db
--- /dev/null
+++ b/filter-kalman/CMakeLists.txt
@@ -0,0 +1,4 @@
+find_package(OpenCV 3.0)
+opentrack_boilerplate(opentrack-filter-kalman)
+target_link_libraries(opentrack-filter-kalman ${OpenCV_LIBS})
+target_include_directories(opentrack-filter-kalman SYSTEM PUBLIC ${OpenCV_INCLUDE_DIRS})
diff --git a/filter-kalman/ftnoir_filter_kalman.h b/filter-kalman/ftnoir_filter_kalman.h
new file mode 100644
index 00000000..a6f40bb7
--- /dev/null
+++ b/filter-kalman/ftnoir_filter_kalman.h
@@ -0,0 +1,73 @@
+#pragma once
+/* 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.
+ */
+#ifndef INCLUDED_FTN_FILTER_H
+#define INCLUDED_FTN_FILTER_H
+
+#include "ui_ftnoir_kalman_filtercontrols.h"
+#include "opentrack/plugin-api.hpp"
+#include <opencv2/core/core.hpp>
+#include <opencv2/video/video.hpp>
+#include <vector>
+#include <QString>
+#include <QElapsedTimer>
+#include <QWidget>
+#include "opentrack/options.hpp"
+using namespace options;
+
+struct settings : opts {
+ value<int> noise_stddev_slider;
+ // slider for noise_stddev goes 0->(mult_noise_stddev * 100)
+ static constexpr double mult_noise_stddev = .5;
+ settings() : opts("kalman-filter"), noise_stddev_slider(b, "noise-stddev", 40)
+ {}
+};
+
+class FTNoIR_Filter : public IFilter
+{
+public:
+ FTNoIR_Filter();
+ void reset();
+ void filter(const double *input, double *output);
+ // Set accel_stddev assuming moving 0.0->accel in dt_ is 3 stddevs: (accel*4/dt_^2)/3.
+ static constexpr double dt_ = .4;
+ static constexpr double accel = 60.;
+ static constexpr double accel_stddev = (accel*4/(dt_*dt_))/3.0;
+ cv::KalmanFilter kalman;
+ double last_input[6];
+ QElapsedTimer timer;
+ settings s;
+ int prev_slider_pos;
+};
+
+class FTNoIR_FilterDll : public Metadata
+{
+public:
+ QString name() { return QString("Kalman"); }
+ QIcon icon() { return QIcon(":/images/filter-16.png"); }
+};
+
+class FilterControls: public IFilterDialog
+{
+ Q_OBJECT
+public:
+ FilterControls() {
+ ui.setupUi(this);
+ connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK()));
+ connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel()));
+ tie_setting(s.noise_stddev_slider, ui.noise_slider);
+ }
+ Ui::KalmanUICFilterControls ui;
+ void register_filter(IFilter*) override {}
+ void unregister_filter() override {}
+ settings s;
+public slots:
+ void doOK();
+ void doCancel();
+};
+
+#endif
diff --git a/filter-kalman/ftnoir_kalman_filtercontrols.ui b/filter-kalman/ftnoir_kalman_filtercontrols.ui
new file mode 100644
index 00000000..69297102
--- /dev/null
+++ b/filter-kalman/ftnoir_kalman_filtercontrols.ui
@@ -0,0 +1,116 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>KalmanUICFilterControls</class>
+ <widget class="QWidget" name="KalmanUICFilterControls">
+ <property name="windowModality">
+ <enum>Qt::NonModal</enum>
+ </property>
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>288</width>
+ <height>132</height>
+ </rect>
+ </property>
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="windowTitle">
+ <string>Kalman settings</string>
+ </property>
+ <property name="windowIcon">
+ <iconset resource="../facetracknoir/ui-res.qrc">
+ <normaloff>:/images/filter-16.png</normaloff>:/images/filter-16.png</iconset>
+ </property>
+ <property name="layoutDirection">
+ <enum>Qt::LeftToRight</enum>
+ </property>
+ <property name="autoFillBackground">
+ <bool>false</bool>
+ </property>
+ <property name="styleSheet">
+ <string notr="true"/>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout">
+ <item>
+ <widget class="QFrame" name="frame">
+ <property name="frameShape">
+ <enum>QFrame::NoFrame</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ <layout class="QGridLayout" name="gridLayout">
+ <item row="0" column="0">
+ <widget class="QLabel" name="label">
+ <property name="text">
+ <string>Noise standard deviation</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QSlider" name="noise_slider">
+ <property name="singleStep">
+ <number>1</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="tickPosition">
+ <enum>QSlider::TicksAbove</enum>
+ </property>
+ <property name="tickInterval">
+ <number>10</number>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="label_2">
+ <property name="text">
+ <string>Written by Donovan Baarda, 2014</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QDialogButtonBox" name="buttonBox">
+ <property name="standardButtons">
+ <set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <resources>
+ <include location="../facetracknoir/ui-res.qrc"/>
+ </resources>
+ <connections/>
+ <designerdata>
+ <property name="gridDeltaX">
+ <number>10</number>
+ </property>
+ <property name="gridDeltaY">
+ <number>10</number>
+ </property>
+ <property name="gridSnapX">
+ <bool>false</bool>
+ </property>
+ <property name="gridSnapY">
+ <bool>false</bool>
+ </property>
+ <property name="gridVisible">
+ <bool>true</bool>
+ </property>
+ </designerdata>
+ <slots>
+ <slot>startEngineClicked()</slot>
+ <slot>stopEngineClicked()</slot>
+ <slot>cameraSettingsClicked()</slot>
+ </slots>
+</ui>
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)
generated by cgit v1.2.3 (git 2.39.1) at 2025-04-03 10:04:32 +0000