vrpn  07.33
Virtual Reality Peripheral Network
vrpn_OneEuroFilter.h
Go to the documentation of this file.
1 
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 
34 template<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 
70 template<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 };
101 template<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;
170  derivative_filter_type _dxfilt;
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 
213  static void setDxIdentity(value_ptr_type dx) {
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 
const value_filter_return_type filter(scalar_type dt, const value_type x)
scalar_type getMinCutoff() const
vrpn_LowPassFilterQuat value_filter_type
static scalar_type computeDerivativeMagnitude(derivative_value_type const dx)
Scalar value_type[DIMENSION]
vrpn_OneEuroFilter(scalar_type mincutoff, scalar_type beta, scalar_type dcutoff)
Filterable::derivative_filter_type derivative_filter_type
static scalar_type computeDerivativeMagnitude(derivative_value_type const dx)
vrpn_LowPassFilter vrpn_LowPassFilterVec
static void computeDerivative(derivative_value_type dx, value_filter_return_type prev, const value_type current, scalar_type dt)
vrpn_LowPassFilterQuat derivative_filter_type
Filterable::value_type value_type
Filterable::value_ptr_type value_ptr_type
void setMinCutoff(scalar_type mincutoff)
value_filter_type::return_type value_filter_return_type
return_type filter(const value_type x, scalar_type alpha)
return_type filter(const q_type x, vrpn_float64 alpha)
const scalar_type * return_type
vrpn_LowPassFilter< DIMENSION, Scalar > value_filter_type
Filterable::derivative_value_type derivative_value_type
scalar_type getDerivativeCutoff() const
Filterable::scalar_type scalar_type
vrpn_LowPassFilter< DIMENSION, Scalar > derivative_filter_type
void setDerivativeCutoff(scalar_type dcutoff)
Scalar value_type[DIMENSION]
vrpn_OneEuroFilter vrpn_OneEuroFilterVec
void setParams(scalar_type mincutoff, scalar_type beta, scalar_type dcutoff)
value_filter_type::return_type value_filter_return_type
static void computeDerivative(derivative_value_type dx, value_filter_return_type prev, const value_type current, scalar_type dt)
return_type hatxprev()
value_filter_type::return_type value_filter_return_type
static void setDxIdentity(value_ptr_type dx)
void setBeta(scalar_type beta)
scalar_type getBeta() const
vrpn_OneEuroFilter< vrpn_QuatFilterable > vrpn_OneEuroFilterQuat
Filterable::value_filter_type value_filter_type
static void setDxIdentity(value_ptr_type dx)