From 4b94d83c1134c7de899d76399e8fa8e8d1a7eaf2 Mon Sep 17 00:00:00 2001
From: Mateo de Mayo <mateo.demayo@collabora.com>
Date: Mon, 14 Feb 2022 16:34:26 -0300
Subject: [PATCH] a/math: Fix one euro implementations not dividing by dt

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.
---
 src/xrt/auxiliary/math/m_filter_one_euro.c    | 15 +++++++++++----
 src/xrt/auxiliary/tracking/t_tracker_slam.cpp |  2 +-
 src/xrt/drivers/ht/ht_driver.hpp              |  4 ++--
 3 files changed, 14 insertions(+), 7 deletions(-)

diff --git a/src/xrt/auxiliary/math/m_filter_one_euro.c b/src/xrt/auxiliary/math/m_filter_one_euro.c
index 6297a5c9c..f74a28c33 100644
--- a/src/xrt/auxiliary/math/m_filter_one_euro.c
+++ b/src/xrt/auxiliary/math/m_filter_one_euro.c
@@ -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);
diff --git a/src/xrt/auxiliary/tracking/t_tracker_slam.cpp b/src/xrt/auxiliary/tracking/t_tracker_slam.cpp
index 5e23653cc..61827ec55 100644
--- a/src/xrt/auxiliary/tracking/t_tracker_slam.cpp
+++ b/src/xrt/auxiliary/tracking/t_tracker_slam.cpp
@@ -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;
 };
diff --git a/src/xrt/drivers/ht/ht_driver.hpp b/src/xrt/drivers/ht/ht_driver.hpp
index 6058c77e8..4f0b37b63 100644
--- a/src/xrt/drivers/ht/ht_driver.hpp
+++ b/src/xrt/drivers/ht/ht_driver.hpp
@@ -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;