mirrors/monado
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/monado/monado.git synced 2025-01-28 17:38:27 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

a/math: Fix one euro implementations not dividing by dt

Browse source 
The dy name was kept. It can now be thought of as "[d]erivative of [y]"
instead of the previous "change of y". Also, the beta parameters of the
one-euro users were adjusted by approximately multiplying them by 0.016
to compensate.
This commit is contained in:
Mateo de Mayo 2022-02-14 16:34:26 -03:00 committed by Jakob Bornecrantz
parent 17dee2bb68
commit 4b94d83c11
3 changed files with 14 additions and 7 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

15
src/xrt/auxiliary/math/m_filter_one_euro.c
View file

@ -146,7 +146,7 @@ m_filter_f32_run(struct m_filter_euro_f32 *f, uint64_t ts, const float *in_y, fl
double dt = 0;
double alpha_d = filter_one_euro_compute_alpha_d(&f->base, &dt, ts, true);
double dy = *in_y - f->prev_y;
double dy = (*in_y - f->prev_y) / dt;
/* Smooth the dy values and use them to calculate the frequency cutoff for the main filter */
f->prev_dy = exp_smooth(alpha_d, dy, f->prev_dy);
@ -178,7 +178,7 @@ m_filter_euro_vec2_run(struct m_filter_euro_vec2 *f, uint64_t ts, const struct x
double dt = 0;
double alpha_d = filter_one_euro_compute_alpha_d(&f->base, &dt, ts, true);
struct xrt_vec2 dy = m_vec2_sub((*in_y), f->prev_y);
struct xrt_vec2 dy = m_vec2_div_scalar(m_vec2_sub((*in_y), f->prev_y), dt);
f->prev_dy = exp_smooth_vec2(alpha_d, dy, f->prev_dy);
double dy_mag = m_vec2_len(f->prev_dy);
@ -205,7 +205,7 @@ m_filter_euro_vec2_run_no_commit(struct m_filter_euro_vec2 *f,
double dt = 0;
double alpha_d = filter_one_euro_compute_alpha_d(&f->base, &dt, ts, false);
struct xrt_vec2 dy = m_vec2_sub((*in_y), f->prev_y);
struct xrt_vec2 dy = m_vec2_div_scalar(m_vec2_sub((*in_y), f->prev_y), dt);
struct xrt_vec2 prev_dy = exp_smooth_vec2(alpha_d, dy, f->prev_dy);
double dy_mag = m_vec2_len(prev_dy);
@ -237,7 +237,7 @@ m_filter_euro_vec3_run(struct m_filter_euro_vec3 *f, uint64_t ts, const struct x
double dt = 0;
double alpha_d = filter_one_euro_compute_alpha_d(&f->base, &dt, ts, true);
struct xrt_vec3 dy = m_vec3_sub((*in_y), f->prev_y);
struct xrt_vec3 dy = m_vec3_div_scalar(m_vec3_sub((*in_y), f->prev_y), dt);
f->prev_dy = exp_smooth_vec3(alpha_d, dy, f->prev_dy);
double dy_mag = m_vec3_len(f->prev_dy);
@ -271,6 +271,13 @@ m_filter_euro_quat_run(struct m_filter_euro_quat *f, uint64_t ts, const struct x
struct xrt_quat dy;
math_quat_unrotate(&f->prev_y, in_y, &dy);
// Scale dy with dt through a conversion to angle_axis
struct xrt_vec3 dy_aa;
math_quat_ln(&dy, &dy_aa);
dy_aa = m_vec3_div_scalar(dy_aa, dt);
math_quat_exp(&dy_aa, &dy);
f->prev_dy = exp_smooth_quat(alpha_d, dy, f->prev_dy);
double dy_mag = math_quat_len(&f->prev_dy);

2
src/xrt/auxiliary/tracking/t_tracker_slam.cpp
View file

@ -243,7 +243,7 @@ struct TrackerSlam
m_filter_euro_quat rot_oe; //!< One euro rotation filter
const float min_cutoff = M_PI; //!< Default minimum cutoff frequency
const float min_dcutoff = 1; //!< Default minimum cutoff frequency for the derivative
const float beta = 1; //!< Default speed coefficient
const float beta = 0.16; //!< Default speed coefficient
} filter;
};

4
src/xrt/drivers/ht/ht_driver.hpp
View file

@ -69,13 +69,13 @@ using namespace xrt::auxiliary::util;
#define FCMIN_BBOX_POSITION 30.0f
#define FCMIN_D_BB0X_POSITION 25.0f
#define BETA_BB0X_POSITION 0.6f
#define BETA_BB0X_POSITION 0.01f
#define FCMIN_HAND 4.0f
#define FCMIN_D_HAND 12.0f
#define BETA_HAND 0.05f
#define BETA_HAND 0.0083f
class ht_model;