diff options
| -rw-r--r-- | src/argaze/utils/tobii_stream_arcube_display.py | 21 |
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) |