summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--cmake/FindEigen3.cmake81
-rw-r--r--filter-kalman/CMakeLists.txt9
-rw-r--r--filter-kalman/ftnoir_filter_kalman.h75
-rw-r--r--filter-kalman/ftnoir_kalman_filtercontrols.ui94
-rw-r--r--filter-kalman/kalman.cpp338
-rw-r--r--filter-kalman/kalman.h177
6 files changed, 588 insertions, 186 deletions
diff --git a/cmake/FindEigen3.cmake b/cmake/FindEigen3.cmake
new file mode 100644
index 00000000..fc29db7e
--- /dev/null
+++ b/cmake/FindEigen3.cmake
@@ -0,0 +1,81 @@
+# - Try to find Eigen3 lib
+#
+# This module supports requiring a minimum version, e.g. you can do
+# find_package(Eigen3 3.1.2)
+# to require version 3.1.2 or newer of Eigen3.
+#
+# Once done this will define
+#
+# EIGEN3_FOUND - system has eigen lib with correct version
+# EIGEN3_INCLUDE_DIR - the eigen include directory
+# EIGEN3_VERSION - eigen version
+
+# Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org>
+# Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr>
+# Copyright (c) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
+# Redistribution and use is allowed according to the terms of the 2-clause BSD license.
+
+if(NOT Eigen3_FIND_VERSION)
+ if(NOT Eigen3_FIND_VERSION_MAJOR)
+ set(Eigen3_FIND_VERSION_MAJOR 2)
+ endif(NOT Eigen3_FIND_VERSION_MAJOR)
+ if(NOT Eigen3_FIND_VERSION_MINOR)
+ set(Eigen3_FIND_VERSION_MINOR 91)
+ endif(NOT Eigen3_FIND_VERSION_MINOR)
+ if(NOT Eigen3_FIND_VERSION_PATCH)
+ set(Eigen3_FIND_VERSION_PATCH 0)
+ endif(NOT Eigen3_FIND_VERSION_PATCH)
+
+ set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}")
+endif(NOT Eigen3_FIND_VERSION)
+
+macro(_eigen3_check_version)
+ file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header)
+
+ string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}")
+ set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}")
+ string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}")
+ set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}")
+ string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}")
+ set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}")
+
+ set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION})
+ if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+ set(EIGEN3_VERSION_OK FALSE)
+ else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+ set(EIGEN3_VERSION_OK TRUE)
+ endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+
+ if(NOT EIGEN3_VERSION_OK)
+
+ message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, "
+ "but at least version ${Eigen3_FIND_VERSION} is required")
+ endif(NOT EIGEN3_VERSION_OK)
+endmacro(_eigen3_check_version)
+
+if (EIGEN3_INCLUDE_DIR)
+
+ # in cache already
+ _eigen3_check_version()
+ set(EIGEN3_FOUND ${EIGEN3_VERSION_OK})
+
+else (EIGEN3_INCLUDE_DIR)
+
+ find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
+ PATHS
+ ${CMAKE_INSTALL_PREFIX}/include
+ ${KDE4_INCLUDE_DIR}
+ PATH_SUFFIXES eigen3 eigen
+ )
+
+ if(EIGEN3_INCLUDE_DIR)
+ _eigen3_check_version()
+ endif(EIGEN3_INCLUDE_DIR)
+
+ #include(FindPackageHandleStandardArgs)
+ #find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK)
+
+ mark_as_advanced(EIGEN3_INCLUDE_DIR)
+
+endif(EIGEN3_INCLUDE_DIR)
+
diff --git a/filter-kalman/CMakeLists.txt b/filter-kalman/CMakeLists.txt
index f84cb0a9..1d1c6412 100644
--- a/filter-kalman/CMakeLists.txt
+++ b/filter-kalman/CMakeLists.txt
@@ -1,6 +1,5 @@
-find_package(OpenCV 3.0 QUIET)
-if(OpenCV_FOUND)
+find_package(Eigen3 QUIET)
+if(EIGEN3_FOUND)
opentrack_boilerplate(opentrack-filter-kalman)
- target_link_libraries(opentrack-filter-kalman ${OpenCV_LIBS})
- target_include_directories(opentrack-filter-kalman SYSTEM PUBLIC ${OpenCV_INCLUDE_DIRS})
-endif() \ No newline at end of file
+ target_include_directories(opentrack-filter-kalman SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIR})
+endif()
diff --git a/filter-kalman/ftnoir_filter_kalman.h b/filter-kalman/ftnoir_filter_kalman.h
deleted file mode 100644
index aa6c978c..00000000
--- a/filter-kalman/ftnoir_filter_kalman.h
+++ /dev/null
@@ -1,75 +0,0 @@
-#pragma once
-/* Copyright (c) 2013 Stanislaw 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.hpp>
-#include <opencv2/video/tracking.hpp>
-#include <vector>
-#include <QString>
-#include <QElapsedTimer>
-#include <QWidget>
-#include "opentrack-compat/options.hpp"
-using namespace options;
-#include "opentrack-compat/timer.hpp"
-
-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];
- Timer timer;
- bool first_run;
- 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
index df213815..6ec18ba8 100644
--- a/filter-kalman/ftnoir_kalman_filtercontrols.ui
+++ b/filter-kalman/ftnoir_kalman_filtercontrols.ui
@@ -9,8 +9,8 @@
<rect>
<x>0</x>
<y>0</y>
- <width>288</width>
- <height>132</height>
+ <width>438</width>
+ <height>141</height>
</rect>
</property>
<property name="sizePolicy">
@@ -37,34 +37,95 @@
</property>
<layout class="QVBoxLayout" name="verticalLayout">
<item>
- <widget class="QFrame" name="frame">
- <property name="frameShape">
- <enum>QFrame::NoFrame</enum>
+ <widget class="QGroupBox" name="groupBox_2">
+ <property name="title">
+ <string>Measurement Noise</string>
</property>
- <property name="frameShadow">
- <enum>QFrame::Raised</enum>
- </property>
- <layout class="QGridLayout" name="gridLayout">
+ <layout class="QGridLayout" name="gridLayout_2">
<item row="0" column="0">
<widget class="QLabel" name="label">
<property name="text">
- <string>Noise standard deviation</string>
+ <string>Rotation</string>
</property>
</widget>
</item>
<item row="0" column="1">
- <widget class="QSlider" name="noise_slider">
+ <widget class="QSlider" name="noiseRotSlider">
+ <property name="maximum">
+ <number>400</number>
+ </property>
<property name="singleStep">
<number>1</number>
</property>
+ <property name="pageStep">
+ <number>100</number>
+ </property>
+ <property name="tracking">
+ <bool>true</bool>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="invertedAppearance">
+ <bool>false</bool>
+ </property>
+ <property name="tickPosition">
+ <enum>QSlider::TicksBelow</enum>
+ </property>
+ <property name="tickInterval">
+ <number>100</number>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QLabel" name="label_4">
+ <property name="text">
+ <string>Position</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <widget class="QSlider" name="noisePosSlider">
+ <property name="maximum">
+ <number>400</number>
+ </property>
+ <property name="pageStep">
+ <number>100</number>
+ </property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="tickPosition">
- <enum>QSlider::TicksAbove</enum>
+ <enum>QSlider::TicksBelow</enum>
</property>
<property name="tickInterval">
- <number>10</number>
+ <number>100</number>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="2">
+ <widget class="QLabel" name="noiseRotLabel">
+ <property name="minimumSize">
+ <size>
+ <width>65</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>°</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="2">
+ <widget class="QLabel" name="noisePosLabel">
+ <property name="minimumSize">
+ <size>
+ <width>65</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>-</string>
</property>
</widget>
</item>
@@ -72,13 +133,6 @@
</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>
diff --git a/filter-kalman/kalman.cpp b/filter-kalman/kalman.cpp
index 1f23ed90..6ed5ca91 100644
--- a/filter-kalman/kalman.cpp
+++ b/filter-kalman/kalman.cpp
@@ -1,81 +1,220 @@
-/* Copyright (c) 2013 Stanislaw Halik <sthalik@misaki.pl>
+/* Copyright (c) 2016 Michael Welter <mw.pub@welter-4d.de>
*
* 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 "kalman.h"
#include <QDebug>
#include <cmath>
-constexpr double settings::mult_noise_stddev;
+constexpr double settings::adaptivity_window_length;
+constexpr double settings::deadzone_scale;
+constexpr double settings::deadzone_exponent;
+constexpr double settings::process_sigma_pos;
+constexpr double settings::process_simga_rot;
+
+void KalmanFilter::init()
+{
+ // allocate and initialize matrices
+ measurement_noise_cov = MeasureMatrix::Zero();
+ process_noise_cov = StateMatrix::Zero();
+ state_cov = StateMatrix::Zero();
+ state_cov_prior = StateMatrix::Zero();
+ transition_matrix = StateMatrix::Zero();
+ measurement_matrix = StateToMeasureMatrix::Zero();
+ kalman_gain = MeasureToStateMatrix::Zero();
+ // initialize state variables
+ state = StateVector::Zero();
+ state_prior = StateVector::Zero();
+ innovation = PoseVector::Zero();
+}
+
+
+void KalmanFilter::time_update()
+{
+ state_prior = transition_matrix * state;
+ state_cov_prior = transition_matrix * state_cov * transition_matrix.transpose() + process_noise_cov;
+}
+
+
+void KalmanFilter::measurement_update(const PoseVector &measurement)
+{
+ MeasureMatrix tmp = measurement_matrix * state_cov_prior * measurement_matrix.transpose() + measurement_noise_cov;
+ MeasureMatrix tmp_inv = tmp.inverse();
+ kalman_gain = state_cov_prior * measurement_matrix.transpose() * tmp_inv;
+ innovation = measurement - measurement_matrix * state_prior;
+ state = state_prior + kalman_gain * innovation;
+ state_cov = state_cov_prior - kalman_gain * measurement_matrix * state_cov_prior;
+}
+
+
+
+void KalmanProcessNoiseScaler::init()
+{
+ base_cov = StateMatrix::Zero(NUM_STATE_DOF, NUM_STATE_DOF);
+ innovation_cov_estimate = MeasureMatrix::Zero(NUM_MEASUREMENT_DOF, NUM_MEASUREMENT_DOF);
+}
+
+
+/* Uses
+ innovation, measurement_matrix, measurement_noise_cov, and state_cov_prior
+ found in KalmanFilter. It sets
+ process_noise_cov
+*/
+void KalmanProcessNoiseScaler::update(KalmanFilter &kf, double dt)
+{
+ MeasureMatrix ddT = kf.innovation * kf.innovation.transpose();
+ double f = dt / (dt + settings::adaptivity_window_length);
+ innovation_cov_estimate =
+ f * ddT + (1. - f) * innovation_cov_estimate;
+
+ double T1 = (innovation_cov_estimate - kf.measurement_noise_cov).trace();
+ double T2 = (kf.measurement_matrix * kf.state_cov_prior * kf.measurement_matrix.transpose()).trace();
+ double alpha = 0.001;
+ if (T2 > 0. && T1 > 0.)
+ {
+ alpha = T1 / T2;
+ alpha = std::sqrt(alpha);
+ alpha = std::min(1000., std::max(0.001, alpha));
+ }
+ kf.process_noise_cov = alpha * base_cov;
+ //qDebug() << "alpha = " << alpha;
+}
+
+
+PoseVector DeadzoneFilter::filter(const PoseVector &input)
+{
+ PoseVector out;
+ for (int i = 0; i < input.rows(); ++i)
+ {
+ const double dz = dz_size[i];
+ if (dz > 0.)
+ {
+ const double delta = input[i] - last_output[i];
+ const double f = std::pow(std::fabs(delta) / dz, settings::deadzone_exponent);
+ const double response = f / (f + 1.) * delta;
+ out[i] = last_output[i] + response;
+ }
+ else
+ out[i] = input[i];
+ last_output[i] = out[i];
+ }
+ return out;
+}
+
+
+void FTNoIR_Filter::fill_transition_matrix(double dt)
+{
+ for (int i = 0; i < 6; ++i)
+ {
+ kf.transition_matrix(i, i + 6) = dt;
+ }
+}
+
+void FTNoIR_Filter::fill_process_noise_cov_matrix(StateMatrix &target, double dt) const
+{
+ // This model is like movement at fixed velocity plus superimposed
+ // brownian motion. Unlike standard models for tracking of objects
+ // with a very well predictable trajectory (e.g.
+ // https://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical)
+ double sigma_pos = s.process_sigma_pos;
+ double sigma_angle = s.process_simga_rot;
+ double a_pos = sigma_pos * sigma_pos * dt;
+ double a_ang = sigma_angle * sigma_angle * dt;
+ static constexpr double b = 20;
+ static constexpr double c = 1.;
+ for (int i = 0; i < 3; ++i)
+ {
+ target(i, i) = a_pos;
+ target(i, i + 6) = a_pos * c;
+ target(i + 6, i) = a_pos * c;
+ target(i + 6, i + 6) = a_pos * b;
+ }
+ for (int i = 3; i < 6; ++i)
+ {
+ target(i, i) = a_ang;
+ target(i, i + 6) = a_ang * c;
+ target(i + 6, i) = a_ang * c;
+ target(i + 6, i + 6) = a_ang * b;
+ }
+}
+
+
+PoseVector FTNoIR_Filter::do_kalman_filter(const PoseVector &input, double dt, bool new_input)
+{
+ if (new_input)
+ {
+ dt = dt_since_last_input;
+ fill_transition_matrix(dt);
+ fill_process_noise_cov_matrix(kf_adaptive_process_noise_cov.base_cov, dt);
+ kf_adaptive_process_noise_cov.update(kf, dt);
+ kf.time_update();
+ kf.measurement_update(input);
+ }
+ return kf.state.head(6);
+}
+
+
FTNoIR_Filter::FTNoIR_Filter() {
reset();
- prev_slider_pos = s.noise_stddev_slider;
}
-// the following was written by Donovan Baarda <abo@minkirri.apana.org.au>
+// The original code 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));
+void FTNoIR_Filter::reset()
+{
+ kf.init();
+ kf_adaptive_process_noise_cov.init();
+ for (int i = 0; i < 6; ++i)
+ {
+ // initialize part of the transition matrix that do not change.
+ kf.transition_matrix(i, i) = 1.;
+ kf.transition_matrix(i + 6, i + 6) = 1.;
+ // "extract" positions, i.e. the first 6 state dof.
+ kf.measurement_matrix(i, i) = 1.;
+ }
+
+ double noise_variance_position = settings::map_slider_value(s.noise_pos_slider_value);
+ double noise_variance_angle = settings::map_slider_value(s.noise_rot_slider_value);
+ for (int i = 0; i < 3; ++i)
+ {
+ kf.measurement_noise_cov(i , i ) = noise_variance_position;
+ kf.measurement_noise_cov(i + 3, i + 3) = noise_variance_angle;
+ }
+
+ fill_transition_matrix(0.03);
+ fill_process_noise_cov_matrix(kf_adaptive_process_noise_cov.base_cov, 0.03);
+
+ kf.process_noise_cov = kf_adaptive_process_noise_cov.base_cov;
+ kf.state_cov = kf.process_noise_cov;
+
for (int i = 0; i < 6; i++) {
last_input[i] = 0;
}
first_run = true;
+ dt_since_last_input = 0;
+
+ prev_slider_pos[0] = static_cast<slider_value>(s.noise_pos_slider_value);
+ prev_slider_pos[1] = static_cast<slider_value>(s.noise_rot_slider_value);
+
+ dz_filter.reset();
}
-void FTNoIR_Filter::filter(const double* input, double *output)
+
+void FTNoIR_Filter::filter(const double* input_, double *output_)
{
- if (prev_slider_pos != s.noise_stddev_slider)
+ // almost non-existent cost, so might as well ...
+ Eigen::Map<const PoseVector> input(input_, PoseVector::RowsAtCompileTime, 1);
+ Eigen::Map<PoseVector> output(output_, PoseVector::RowsAtCompileTime, 1);
+
+ if (!(prev_slider_pos[0] == s.noise_pos_slider_value &&
+ prev_slider_pos[1] == s.noise_rot_slider_value))
{
reset();
- prev_slider_pos = s.noise_stddev_slider;
}
+
// Start the timer on first filter evaluation.
if (first_run)
{
@@ -84,54 +223,81 @@ void FTNoIR_Filter::filter(const double* input, double *output)
return;
}
+ // 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 = input.cwiseNotEqual(last_input).any();
+
// Get the time in seconds since last run and restart the timer.
const double dt = timer.elapsed_seconds();
+ dt_since_last_input += dt;
timer.start();
- // 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);
+ output = do_kalman_filter(input, dt, new_input);
+
+ {
+ // Compute deadzone size base on estimated state variance.
+ // Given a constant input plus measurement noise, KF should converge to the true input.
+ // This works well. That is the output pose becomes very still afte some time.
+ // The QScaling adaptive filter makes the state cov vary depending on the estimated noise
+ // and the measured noise of the innovation sequence. After a sudden movement it peaks
+ // and then decays asymptotically to some constant value taken in stationary state.
+ // We can use this to calculate the size of the deadzone, so that in the stationary state the
+ // deadzone size is small. Thus the tracking error due to the dz-filter becomes also small.
+ PoseVector variance = kf.state_cov.diagonal().head(6);
+ dz_filter.dz_size = variance.cwiseSqrt() * s.deadzone_scale;
}
- // Set output to the next_output.
- for (int i = 0; i < 6; i++) {
- output[i] = next_output.at<double>(i);
+ output = dz_filter.filter(output);
+
+ if (new_input)
+ {
+ dt_since_last_input = 0;
+ last_input = input;
}
}
+
+
+FilterControls::FilterControls()
+ : filter(nullptr)
+{
+ ui.setupUi(this);
+ connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK()));
+ connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel()));
+
+ tie_setting(s.noise_rot_slider_value, ui.noiseRotSlider);
+ tie_setting(s.noise_pos_slider_value, ui.noisePosSlider);
+
+ connect(&s.noise_rot_slider_value, SIGNAL(valueChanged(const slider_value&)), this, SLOT(updateLabels(const slider_value&)));
+ connect(&s.noise_pos_slider_value, SIGNAL(valueChanged(const slider_value&)), this, SLOT(updateLabels(const slider_value&)));
+
+ updateLabels(slider_value());
+}
+
+
+void FilterControls::updateLabels(const slider_value&)
+{
+ // M$ hates unicode! (M$ autoconverts source code of one kind of utf-8 format,
+ // the one without BOM, to another kind that QT does not like)
+ // Previous attempt to use c++11 utf8 strings like u8" °" now failed for unkown
+ // reasons where it worked before. Hence fallback to QChar(0x00b0).
+ this->ui.noiseRotLabel->setText(
+ QString::number(settings::map_slider_value(s.noise_rot_slider_value), 'f', 3) + " " + QChar(0x00b0));
+
+ this->ui.noisePosLabel->setText(
+ QString::number(settings::map_slider_value(s.noise_pos_slider_value), 'f', 3) + " cm");
+}
+
+
void FilterControls::doOK() {
s.b->save();
close();
}
+
void FilterControls::doCancel()
{
close();
}
+
OPENTRACK_DECLARE_FILTER(FTNoIR_Filter, FilterControls, FTNoIR_FilterDll)
diff --git a/filter-kalman/kalman.h b/filter-kalman/kalman.h
new file mode 100644
index 00000000..aaed3bc1
--- /dev/null
+++ b/filter-kalman/kalman.h
@@ -0,0 +1,177 @@
+#pragma once
+/* Copyright (c) 2016 Michael Welter <mw.pub@welter-4d.de>
+ *
+ * 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 "ui_ftnoir_kalman_filtercontrols.h"
+#include "opentrack/plugin-api.hpp"
+#include "opentrack-compat/options.hpp"
+using namespace options;
+#include "opentrack-compat/timer.hpp"
+
+#include <Eigen/Core>
+#include <Eigen/LU>
+
+#include <QString>
+#include <QWidget>
+
+#include <atomic>
+
+static constexpr int NUM_STATE_DOF = 12;
+static constexpr int NUM_MEASUREMENT_DOF = 6;
+// These vectors are compile time fixed size, stack allocated
+using StateToMeasureMatrix = Eigen::Matrix<double, NUM_MEASUREMENT_DOF, NUM_STATE_DOF>;
+using StateMatrix = Eigen::Matrix<double, NUM_STATE_DOF, NUM_STATE_DOF>;
+using MeasureToStateMatrix = Eigen::Matrix<double, NUM_STATE_DOF, NUM_MEASUREMENT_DOF>;
+using MeasureMatrix = Eigen::Matrix<double, NUM_MEASUREMENT_DOF, NUM_MEASUREMENT_DOF>;
+using StateVector = Eigen::Matrix<double, NUM_STATE_DOF, 1>;
+using PoseVector = Eigen::Matrix<double, NUM_MEASUREMENT_DOF, 1>;
+
+struct KalmanFilter
+{
+ MeasureMatrix
+ measurement_noise_cov;
+ StateMatrix
+ process_noise_cov,
+ state_cov,
+ state_cov_prior,
+ transition_matrix;
+ MeasureToStateMatrix
+ kalman_gain;
+ StateToMeasureMatrix
+ measurement_matrix;
+ StateVector
+ state,
+ state_prior;
+ PoseVector
+ innovation;
+ void init();
+ void time_update();
+ void measurement_update(const PoseVector &measurement);
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+struct KalmanProcessNoiseScaler
+{
+ MeasureMatrix
+ innovation_cov_estimate;
+ StateMatrix
+ base_cov; // baseline (unscaled) process noise covariance matrix
+ void init();
+ void update(KalmanFilter &kf, double dt);
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+
+struct DeadzoneFilter
+{
+ PoseVector
+ last_output,
+ dz_size;
+ DeadzoneFilter() :
+ last_output(PoseVector::Zero()),
+ dz_size(PoseVector::Zero())
+ {}
+ void reset() {
+ last_output = PoseVector::Zero();
+ }
+ PoseVector filter(const PoseVector &input);
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+
+struct settings : opts {
+ value<slider_value> noise_rot_slider_value;
+ value<slider_value> noise_pos_slider_value;
+
+ static constexpr double adaptivity_window_length = 0.5; // seconds
+ static constexpr double deadzone_scale = 2;
+ static constexpr double deadzone_exponent = 2.0;
+ static constexpr double process_sigma_pos = 0.5;
+ static constexpr double process_simga_rot = 0.5;
+
+ static double map_slider_value(const slider_value &v_)
+ {
+ const double v = v_;
+#if 0
+ //return std::pow(10., v * 4. - 3.);
+#else
+ constexpr int min_log10 = -3;
+ constexpr int max_log10 = 1;
+ constexpr int num_divisions = max_log10 - min_log10;
+ /* ascii art representation of slider
+ // ----- // ------// ------// ------- // 4 divisions
+ -3 - 2 -1 0 1 power of 10
+ | |
+ | f + left_side_log10
+ |
+ left_side_log10
+ */
+ const int k = v * num_divisions; // in which division are we?!
+ const double f = v * num_divisions - k; // where in the division are we?!
+ const double ff = f * 9. + 1.;
+ const double multiplier = int(ff * 10.) / 10.;
+ const int left_side_log10 = min_log10 + k;
+ const double val = std::pow(10., left_side_log10) * multiplier;
+ return val;
+#endif
+ }
+
+ settings() :
+ opts("kalman-filter"),
+ noise_rot_slider_value(b, "noise-rotation-slider", slider_value(0.5, 0., 1.)),
+ noise_pos_slider_value(b, "noise-position-slider", slider_value(0.5, 0., 1.))
+ {}
+
+};
+
+class FTNoIR_Filter : public IFilter
+{
+ PoseVector do_kalman_filter(const PoseVector &input, double dt, bool new_input);
+ void fill_transition_matrix(double dt);
+ void fill_process_noise_cov_matrix(StateMatrix &target, double dt) const;
+public:
+ FTNoIR_Filter();
+ void reset();
+ void filter(const double *input, double *output);
+ PoseVector last_input;
+ Timer timer;
+ bool first_run;
+ double dt_since_last_input;
+ settings s;
+ KalmanFilter kf;
+ KalmanProcessNoiseScaler kf_adaptive_process_noise_cov;
+ DeadzoneFilter dz_filter;
+ slider_value prev_slider_pos[2];
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+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::KalmanUICFilterControls ui;
+ void register_filter(IFilter*) override {}
+ void unregister_filter() override {}
+ settings s;
+ FTNoIR_Filter *filter;
+public slots:
+ void doOK();
+ void doCancel();
+ void updateLabels(const slider_value&);
+};