1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
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)
|
