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
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
|
/* Copyright (c) 2012-2013 Stanislaw Halik
*
* 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_accela/ftnoir_filter_accela.h"
#include <algorithm>
#include <cmath>
#include <QDebug>
#include <QMutexLocker>
#include "facetracknoir/global-settings.h"
using namespace std;
FTNoIR_Filter::FTNoIR_Filter()
{
first_run = true;
loadSettings();
}
FTNoIR_Filter::~FTNoIR_Filter()
{
}
void FTNoIR_Filter::loadSettings() {
QSettings settings("opentrack"); // Registry settings (in HK_USER)
QString currentFile = settings.value ( "SettingsFile", QCoreApplication::applicationDirPath() + "/settings/default.ini" ).toString();
QSettings iniFile( currentFile, QSettings::IniFormat ); // Application settings (in INI-file)
iniFile.beginGroup ( "Accela" );
zoom_factor = iniFile.value("zoom-slowness", ACCELA_ZOOM_SLOWNESS).toDouble();
rotation_alpha = iniFile.value("rotation-alpha", ACCELA_SMOOTHING_ROTATION).toDouble();
translation_alpha = iniFile.value("translation-alpha", ACCELA_SMOOTHING_TRANSLATION).toDouble();
second_order_alpha = iniFile.value("second-order-alpha", ACCELA_SECOND_ORDER_ALPHA).toDouble();
third_order_alpha = iniFile.value("third-order-alpha", ACCELA_THIRD_ORDER_ALPHA).toDouble();
deadzone = iniFile.value("deadzone", 0.0).toDouble();
// bigger means less filtering
static const double init_scaling[] = {
1, // X
1, // Y
1, // Z
1, // Yaw
1, // Pitch
1 // Roll
};
for (int i = 0; i < 6; i++)
{
scaling[i] = iniFile.value(QString("axis-%1").arg(QString::number(i)), init_scaling[i]).toDouble();
}
expt = iniFile.value("exponent", 2.0).toDouble();
iniFile.endGroup();
}
void FTNoIR_Filter::receiveSettings(double rot, double trans, double zoom_fac, double dz, double exponent)
{
QMutexLocker foo(&mutex);
rotation_alpha = rot;
translation_alpha = trans;
zoom_factor = zoom_fac;
deadzone = dz;
expt = exponent;
}
static inline double parabola(const double a, const double x, const double dz, const double expt)
{
const double sign = x > 0 ? 1 : -1;
const double a1 = 1./a;
return a1 * pow(std::max<double>(fabs(x) - dz, 1e-3), expt) * sign;
}
template<typename T>
static inline T clamp(const T min, const T max, const T value)
{
if (value < min)
return min;
if (value > max)
return max;
return value;
}
void FTNoIR_Filter::FilterHeadPoseData(const double* target_camera_position,
double *new_camera_position,
const double* last_post_filter_values)
{
if (first_run)
{
for (int i = 0; i < 6; i++)
{
new_camera_position[i] = target_camera_position[i];
last_input[i] = target_camera_position[i];
for (int j = 0; j < 3; j++)
last_output[j][i] = target_camera_position[i];
}
timer.start();
frame_delta = 1;
first_run = false;
return;
}
bool new_frame = false;
for (int i = 0; i < 6; i++)
{
if (target_camera_position[i] != last_input[i])
{
new_frame = true;
break;
}
}
if (new_frame)
{
for (int i = 0; i < 6; i++)
last_input[i] = target_camera_position[i];
frame_delta = timer.isValid() ? timer.elapsed() : 1;
timer.start();
} else {
auto d = timer.elapsed();
double c = clamp(0.0, 1.0, d / (double) frame_delta);
QMutexLocker foo(&mutex);
for (int i = 0; i < 6; i++)
new_camera_position[i] =
last_output[1][i] + (last_output[0][i] - last_output[1][i]) * c;
return;
}
QMutexLocker foo(&mutex);
for (int i=0;i<6;i++)
{
const double vec = target_camera_position[i] - last_output[0][i];
const double vec2 = target_camera_position[i] - last_output[1][i];
const double vec3 = target_camera_position[i] - last_output[2][i];
const int sign = vec < 0 ? -1 : 1;
const double a = i >= 3 ? rotation_alpha : translation_alpha;
const double a2 = a * second_order_alpha;
const double a3 = a * third_order_alpha;
const double reduction = 1. / std::max(1., 1. + zoom_factor * -last_post_filter_values[TZ] / 1000);
const double velocity =
parabola(a, vec * scaling[i], deadzone, expt) * reduction +
parabola(a2, vec2 * scaling[i], deadzone, expt) * reduction +
parabola(a3, vec3 * scaling[i], deadzone, expt) * reduction;
const double result = last_output[0][i] + velocity;
const bool done = sign > 0 ? result >= target_camera_position[i] : result <= target_camera_position[i];
new_camera_position[i] = done ? target_camera_position[i] : result;
last_output[2][i] = last_output[1][i];
last_output[1][i] = last_output[0][i];
last_output[0][i] = new_camera_position[i];
}
}
extern "C" FTNOIR_FILTER_BASE_EXPORT IFilter* CALLING_CONVENTION GetConstructor()
{
return new FTNoIR_Filter;
}
|
