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
138
139
|
/* 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>
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 = 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();
}
extern "C" OPENTRACK_EXPORT Metadata* GetMetadata()
{
return new FTNoIR_FilterDll;
}
extern "C" OPENTRACK_EXPORT IFilter* GetConstructor()
{
return new FTNoIR_Filter;
}
extern "C" OPENTRACK_EXPORT IFilterDialog* GetDialog() {
return new FilterControls;
}
|
