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
130
131
132
133
134
135
136
137
|
/* 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.
*/
#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;
}
first_run = true;
}
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 on first filter evaluation.
if (first_run)
{
timer.start();
first_run = false;
return;
}
// Get the time in seconds since last run and restart the timer.
const double dt = timer.elapsed_seconds();
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);
}
// 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()
{
close();
}
OPENTRACK_DECLARE_FILTER(FTNoIR_Filter, FilterControls, FTNoIR_FilterDll)
|
