aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/argaze/utils/tobii_stream_arcube_display.py21
1 files changed, 13 insertions, 8 deletions
diff --git a/src/argaze/utils/tobii_stream_arcube_display.py b/src/argaze/utils/tobii_stream_arcube_display.py
index bed3ba6..d307206 100644
--- a/src/argaze/utils/tobii_stream_arcube_display.py
+++ b/src/argaze/utils/tobii_stream_arcube_display.py
@@ -113,6 +113,7 @@ def main():
last_gyroscope_ts_ms = 0
gyroscope_drift = numpy.zeros(3)
head_rotation = numpy.zeros(3)
+ smooth_factor = 0.5
# Init data timestamped in millisecond
data_ts_ms = 0
@@ -152,8 +153,9 @@ def main():
# Update gyroscope drift smoothly and reset head rotation when gyroscope is stable
if numpy.linalg.norm(gyroscope_derivation) < 1e-5:
- gyroscope_drift = current_gyroscope * 0.1 + gyroscope_drift * 0.9
+ gyroscope_drift = gyroscope_drift * smooth_factor + current_gyroscope * (1 - smooth_factor)
head_rotation = numpy.zeros(3)
+
print(f'> gyroscope_drift={gyroscope_drift}')
# Integrate gyroscope with drift compensation
@@ -237,18 +239,21 @@ def main():
# Cube pose estimation fails: use tobii glasses inertial sensors to estimate cube pose from last estimated pose
elif aruco_cube_success:
- # Translate cube according head translation speed
- #aruco_cube_tvec += head_translation_speed * (video_ts_ms - aruco_cube_ts_ms)
+ # TODO : Translate cube according head translation speed ?
#if numpy.linalg.norm(head_rotation) > 0:
# print(f'X={head_rotation[0]:3f}, Y={head_rotation[1]:3f}, Z={head_rotation[2]:3f}')
# Rotate cube around origin according head rotation
- cam_rot = make_rotation_matrix(*head_rotation)
-
- cube_rot, _ = cv.Rodrigues(aruco_cube_rvec)
- cube_rot = cube_rot.dot(cam_rot)
- new_rvec, _ = cv.Rodrigues(cube_rot)
+ R = make_rotation_matrix(*head_rotation)
+
+ # rotate tvec ???
+ #new_tvec = aruco_cube_tvec.dot(R.T)
+
+ # rotate rvec
+ C, _ = cv.Rodrigues(aruco_cube_rvec)
+ C = C.dot(R)
+ new_rvec, _ = cv.Rodrigues(C)
# Set cube pose estimation
aruco_cube.set_pose(tvec = aruco_cube_tvec, rvec = new_rvec)