vrpn
07.33
Virtual Reality Peripheral Network
|
Go to the documentation of this file.
34 template<
int DIMENSION = 3,
typename Scalar = vrpn_
float64>
47 memcpy(_hatxprev, x,
sizeof(_hatxprev));
51 for (
int i = 0; i < DIMENSION; ++i) {
52 hatx[i] = alpha * x[i] + (1 - alpha) * _hatxprev[i];
55 memcpy(_hatxprev, hatx,
sizeof(_hatxprev));
70 template<
int DIMENSION = 3,
typename Scalar = vrpn_
float64>
82 for (
int i = 0; i < DIMENSION; ++i) {
88 for (
int i = 0; i < DIMENSION; ++i) {
89 dx[i] = (current[i] - prev[i]) / dt;
94 for (
int i = 0; i < DIMENSION; ++i) {
95 sqnorm += dx[i] * dx[i];
97 return sqrt(static_cast<vrpn_float64>(sqnorm));
101 template<
typename Filterable = vrpn_VectorFilterable<> >
115 _mincutoff(mincutoff), _dcutoff(dcutoff),
121 _mincutoff = mincutoff;
139 _mincutoff = mincutoff;
148 Filterable::setDxIdentity(dx);
151 Filterable::computeDerivative(dx, _xfilt.hatxprev(), x, dt);
154 scalar_type derivative_magnitude = Filterable::computeDerivativeMagnitude(_dxfilt.filter(dx, alpha(dt, _dcutoff)));
155 scalar_type cutoff = _mincutoff + _beta * derivative_magnitude;
157 return _xfilt.filter(x, alpha(dt, cutoff));
185 q_copy(_hatxprev, x);
189 q_slerp(hatx, _hatxprev, x, alpha);
190 q_copy(_hatxprev, hatx);
214 dx[Q_X] = dx[Q_Y] = dx[Q_Z] = 0;
222 q_invert(inverse_prev, prev);
223 q_mult(dx, current, inverse_prev);
229 dx[Q_W] = dx[Q_W] * rate + (1.0 - rate);
236 return 2.0 * acos(static_cast<vrpn_float64>(dx[Q_W]));
const value_filter_return_type filter(scalar_type dt, const value_type x)
Filterable::scalar_type scalar_type
q_type derivative_value_type
Filterable::value_ptr_type value_ptr_type
scalar_type getMinCutoff() const
value_filter_type::return_type value_filter_return_type
void setBeta(scalar_type beta)
void setMinCutoff(scalar_type mincutoff)
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
vrpn_LowPassFilter vrpn_LowPassFilterVec
return_type filter(const q_type x, vrpn_float64 alpha)
value_filter_type::return_type value_filter_return_type
Scalar value_type[DIMENSION]
Filterable::derivative_filter_type derivative_filter_type
value_type derivative_value_type
static scalar_type computeDerivativeMagnitude(derivative_value_type const dx)
void setDerivativeCutoff(scalar_type dcutoff)
Filterable::value_type value_type
static void setDxIdentity(value_ptr_type dx)
value_filter_type::return_type value_filter_return_type
scalar_type getDerivativeCutoff() const
vrpn_LowPassFilter< DIMENSION, Scalar > derivative_filter_type
vrpn_OneEuroFilter vrpn_OneEuroFilterVec
vrpn_OneEuroFilter(scalar_type mincutoff, scalar_type beta, scalar_type dcutoff)
Scalar value_type[DIMENSION]
vrpn_LowPassFilterQuat value_filter_type
const typedef double * return_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)
static void setDxIdentity(value_ptr_type dx)
scalar_type getBeta() const
Filterable::derivative_value_type derivative_value_type
void setParams(scalar_type mincutoff, scalar_type beta, scalar_type dcutoff)
vrpn_LowPassFilterQuat derivative_filter_type
vrpn_OneEuroFilter< vrpn_QuatFilterable > vrpn_OneEuroFilterQuat
const typedef scalar_type * return_type
Filterable::value_filter_type value_filter_type
return_type filter(const value_type x, scalar_type alpha)