vrpn 07.35
Virtual Reality Peripheral Network
 
Loading...
Searching...
No Matches
vrpn_OneEuroFilter.h
Go to the documentation of this file.
1
13
14// Copyright Reviatech 2012.
15// Distributed under the Boost Software License, Version 1.0.
16// (See accompanying file LICENSE_1_0.txt or copy at
17// http://www.boost.org/LICENSE_1_0.txt)
18
19#pragma once
20
21// Internal Includes
22#include <quat.h>
23
24// Library/third-party includes
25// - none
26
27// Standard includes
28#include <math.h> // for sqrt() and acos()
29#include <string.h> // for memcpy
30
31// "One Euro" filter for reducing jitter
32// http://hal.inria.fr/hal-00670496/
33
34template<int DIMENSION = 3, typename Scalar = vrpn_float64>
36 public:
37 typedef Scalar scalar_type;
38 typedef Scalar value_type[DIMENSION];
39 typedef const scalar_type * return_type;
40
41 vrpn_LowPassFilter() : _firstTime(true) {
42 }
43
45 if (_firstTime) {
46 _firstTime = false;
47 memcpy(_hatxprev, x, sizeof(_hatxprev));
48 }
49
50 value_type hatx;
51 for (int i = 0; i < DIMENSION; ++i) {
52 hatx[i] = alpha * x[i] + (1 - alpha) * _hatxprev[i];
53 }
54
55 memcpy(_hatxprev, hatx, sizeof(_hatxprev));
56 return _hatxprev;
57 }
58
60 return _hatxprev;
61 }
62
63 private:
64 bool _firstTime;
65 value_type _hatxprev;
66};
67
69
70template<int DIMENSION = 3, typename Scalar = vrpn_float64>
72 public:
73 typedef Scalar scalar_type;
74 typedef Scalar value_type[DIMENSION];
76 typedef Scalar * value_ptr_type;
80
81 static void setDxIdentity(value_ptr_type dx) {
82 for (int i = 0; i < DIMENSION; ++i) {
83 dx[i] = 0;
84 }
85 }
86
88 for (int i = 0; i < DIMENSION; ++i) {
89 dx[i] = (current[i] - prev[i]) / dt;
90 }
91 }
93 scalar_type sqnorm = 0;
94 for (int i = 0; i < DIMENSION; ++i) {
95 sqnorm += dx[i] * dx[i];
96 }
97 return sqrt(static_cast<vrpn_float64>(sqnorm));
98 }
99
100};
101template<typename Filterable = vrpn_VectorFilterable<> >
103 public:
104 typedef Filterable contents;
105 typedef typename Filterable::scalar_type scalar_type;
106 typedef typename Filterable::value_type value_type;
107 typedef typename Filterable::derivative_value_type derivative_value_type;
108 typedef typename Filterable::value_ptr_type value_ptr_type;
109 typedef typename Filterable::derivative_filter_type derivative_filter_type;
110 typedef typename Filterable::value_filter_type value_filter_type;
111 typedef typename value_filter_type::return_type value_filter_return_type;
112
114 _firstTime(true),
115 _mincutoff(mincutoff), _dcutoff(dcutoff),
116 _beta(beta) {};
117
118 vrpn_OneEuroFilter() : _firstTime(true), _mincutoff(1), _dcutoff(1), _beta(0.5) {};
119
120 void setMinCutoff(scalar_type mincutoff) {
121 _mincutoff = mincutoff;
122 }
124 return _mincutoff;
125 }
126 void setBeta(scalar_type beta) {
127 _beta = beta;
128 }
130 return _beta;
131 }
133 _dcutoff = dcutoff;
134 }
136 return _dcutoff;
137 }
138 void setParams(scalar_type mincutoff, scalar_type beta, scalar_type dcutoff) {
139 _mincutoff = mincutoff;
140 _beta = beta;
141 _dcutoff = dcutoff;
142 }
145
146 if (_firstTime) {
147 _firstTime = false;
148 Filterable::setDxIdentity(dx);
149
150 } else {
151 Filterable::computeDerivative(dx, _xfilt.hatxprev(), x, dt);
152 }
153
154 scalar_type derivative_magnitude = Filterable::computeDerivativeMagnitude(_dxfilt.filter(dx, alpha(dt, _dcutoff)));
155 scalar_type cutoff = _mincutoff + _beta * derivative_magnitude;
156
157 return _xfilt.filter(x, alpha(dt, cutoff));
158 }
159
160 private:
161 static scalar_type alpha(scalar_type dt, scalar_type cutoff) {
162 scalar_type tau = scalar_type(1) / (scalar_type(2) * Q_PI * cutoff);
163 return scalar_type(1) / (scalar_type(1) + tau / dt);
164 }
165
166 bool _firstTime;
167 scalar_type _mincutoff, _dcutoff;
168 scalar_type _beta;
169 value_filter_type _xfilt;
171};
172
174
176 public:
177 typedef const double * return_type;
178
179 vrpn_LowPassFilterQuat() : _firstTime(true) {
180 }
181
182 return_type filter(const q_type x, vrpn_float64 alpha) {
183 if (_firstTime) {
184 _firstTime = false;
185 q_copy(_hatxprev, x);
186 }
187
188 q_type hatx;
189 q_slerp(hatx, _hatxprev, x, alpha);
190 q_copy(_hatxprev, hatx);
191 return _hatxprev;
192 }
193
195 return _hatxprev;
196 }
197
198 private:
199 bool _firstTime;
200 q_type _hatxprev;
201};
202
204 public:
205 typedef double scalar_type;
206 typedef q_type value_type;
207 typedef q_type derivative_value_type;
208 typedef q_type value_ptr_type;
212
214 dx[Q_X] = dx[Q_Y] = dx[Q_Z] = 0;
215 dx[Q_W] = 1;
216 }
217
219 scalar_type rate = 1.0 / dt;
220
221 q_type inverse_prev;
222 q_invert(inverse_prev, prev);
223 q_mult(dx, current, inverse_prev);
224
225 // nlerp instead of slerp
226 dx[Q_X] *= rate;
227 dx[Q_Y] *= rate;
228 dx[Q_Z] *= rate;
229 dx[Q_W] = dx[Q_W] * rate + (1.0 - rate);
230 q_normalize(dx, dx);
231
232 //q_slerp(dx, identity, dx, 1.0 / dt);
233 }
236 return 2.0 * acos(static_cast<vrpn_float64>(dx[Q_W]));
237 }
238
239};
240
242
243
return_type filter(const q_type x, vrpn_float64 alpha)
Scalar value_type[DIMENSION]
return_type filter(const value_type x, scalar_type alpha)
const scalar_type * return_type
void setDerivativeCutoff(scalar_type dcutoff)
value_filter_type::return_type value_filter_return_type
Filterable::derivative_filter_type derivative_filter_type
Filterable::value_type value_type
Filterable::value_filter_type value_filter_type
scalar_type getBeta() const
Filterable::value_ptr_type value_ptr_type
Filterable::derivative_value_type derivative_value_type
void setMinCutoff(scalar_type mincutoff)
value_filter_return_type filter(scalar_type dt, const value_type x)
void setParams(scalar_type mincutoff, scalar_type beta, scalar_type dcutoff)
void setBeta(scalar_type beta)
vrpn_OneEuroFilter(scalar_type mincutoff, scalar_type beta, scalar_type dcutoff)
Filterable::scalar_type scalar_type
scalar_type getMinCutoff() const
scalar_type getDerivativeCutoff() const
static void setDxIdentity(value_ptr_type dx)
value_filter_type::return_type value_filter_return_type
vrpn_LowPassFilterQuat value_filter_type
static scalar_type computeDerivativeMagnitude(derivative_value_type const dx)
vrpn_LowPassFilterQuat derivative_filter_type
static void computeDerivative(derivative_value_type dx, value_filter_return_type prev, const value_type current, scalar_type dt)
vrpn_LowPassFilter< DIMENSION, Scalar > value_filter_type
value_filter_type::return_type value_filter_return_type
static void setDxIdentity(value_ptr_type dx)
vrpn_LowPassFilter< DIMENSION, Scalar > derivative_filter_type
static void computeDerivative(derivative_value_type dx, value_filter_return_type prev, const value_type current, scalar_type dt)
static scalar_type computeDerivativeMagnitude(derivative_value_type const dx)
Scalar value_type[DIMENSION]
vrpn_OneEuroFilter< vrpn_QuatFilterable > vrpn_OneEuroFilterQuat
vrpn_LowPassFilter vrpn_LowPassFilterVec
vrpn_OneEuroFilter vrpn_OneEuroFilterVec