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
|
/********************************************************************************
* FaceTrackNoIR This program is a private project of some enthusiastic *
* gamers from Holland, who don't like to pay much for *
* head-tracking. *
* *
* Copyright (C) 2012 Wim Vriend (Developing) *
* Ron Hendriks (Researching and Testing) *
* *
* Homepage *
* *
* This program is free software; you can redistribute it and/or modify it *
* under the terms of the GNU General Public License as published by the *
* Free Software Foundation; either version 3 of the License, or (at your *
* option) any later version. *
* *
* This program is distributed in the hope that it will be useful, but *
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY *
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for *
* more details. *
* *
* You should have received a copy of the GNU General Public License along *
* with this program; if not, see <http://www.gnu.org/licenses/>. *
* *
********************************************************************************/
#include "ftnoir_filter_ewma2.h"
#include "math.h"
#include <QDebug>
#include <QWidget>
#include "facetracknoir/global-settings.h"
#include <algorithm>
#include <QMutexLocker>
///////////////////////////////////////////////////////////////////////////////
//
// EWMA Filter: Exponentially Weighted Moving Average filter with dynamic smoothing parameter
//
// This filter tries to adjust the amount of filtering to minimize lag when
// moving, and minimize noise when still. It uses the delta filtered over the
// last 3 frames (0.1secs) compared to the delta's average noise variance over
// the last 3600 frames (~2mins) to try and detect movement vs noise. As the
// delta increases from 0->3 stdevs of the noise, the filtering scales down
// from maxSmooth->minSmooth at a rate controlled by the powCurve setting.
//
// Written by Donovan Baarda
//
///////////////////////////////////////////////////////////////////////////////
FTNoIR_Filter::FTNoIR_Filter() :
first_run(true),
// Deltas are smoothed over the last 3 frames (0.1sec at 30fps).
delta_smoothing(1.0/3.0),
// Noise is smoothed over the last 3600 frames (~2mins at 30fps).
noise_smoothing(1.0/3600.0)
{
}
void FTNoIR_Filter::receiveSettings()
{
s.b->reload();
}
void FTNoIR_Filter::FilterHeadPoseData(const double *target_camera_position,
double *new_camera_position)
{
double new_delta, new_noise, norm_noise;
double alpha;
double pos[6];
//On the first run, initialize to output=target and return.
if (first_run==true) {
for (int i=0;i<6;i++) {
delta[i] = 0.0;
noise[i] = 0.0;
l.write(target_camera_position, target_camera_position, new_camera_position);
}
first_run=false;
return;
}
if (!l.idempotentp(target_camera_position))
{
double cur[6];
l.get_state(cur);
// Calculate the new camera position.
for (int i=0;i<6;i++) {
// Calculate the current and smoothed delta.
new_delta = target_camera_position[i]-cur[i];
delta[i] = delta_smoothing*new_delta + (1.0-delta_smoothing)*delta[i];
// Calculate the current and smoothed noise variance.
new_noise = delta[i]*delta[i];
noise[i] = noise_smoothing*new_noise + (1.0-noise_smoothing)*noise[i];
// Normalise the noise between 0->1 for 0->9 variances (0->3 stddevs).
norm_noise = std::min<double>(new_noise/(9.0*noise[i]), 1.0);
// Calculate the alpha from the normalized noise.
// TODO(abo): change kSmoothingScaleCurve to a float where 1.0 is sqrt(norm_noise).
alpha = 1.0/(s.kMinSmoothing+(1.0-pow(norm_noise,s.kSmoothingScaleCurve/20.0))*(s.kMaxSmoothing-s.kMinSmoothing));
// Update the current camera position to the new position.
double value = alpha*target_camera_position[i] + (1.0-alpha)*cur[i];
pos[i] = value;
}
}
l.write(target_camera_position, pos, new_camera_position);
}
extern "C" FTNOIR_FILTER_BASE_EXPORT IFilter* CALLING_CONVENTION GetConstructor()
{
return new FTNoIR_Filter;
}
|
