aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
authorThéo de la Hogue2022-11-02 20:24:01 +0100
committerThéo de la Hogue2022-11-02 20:24:01 +0100
commit4e78540aef28f5c60620859b354ec400b74c0d55 (patch)
treee418d11290c56c03dfdaeb8a0e2bc26379e50a21 /src
parent76f1391303a921a25435a051ec1c4077f8097822 (diff)
downloadargaze-4e78540aef28f5c60620859b354ec400b74c0d55.zip
argaze-4e78540aef28f5c60620859b354ec400b74c0d55.tar.gz
argaze-4e78540aef28f5c60620859b354ec400b74c0d55.tar.bz2
argaze-4e78540aef28f5c60620859b354ec400b74c0d55.tar.xz
Using imu gyroscope feature integration.
Diffstat (limited to 'src')
-rw-r--r--src/argaze/utils/tobii_stream_arcube_display.py56
1 files changed, 17 insertions, 39 deletions
diff --git a/src/argaze/utils/tobii_stream_arcube_display.py b/src/argaze/utils/tobii_stream_arcube_display.py
index 292c2b4..4ba59af 100644
--- a/src/argaze/utils/tobii_stream_arcube_display.py
+++ b/src/argaze/utils/tobii_stream_arcube_display.py
@@ -44,6 +44,7 @@ def main():
parser.add_argument('-t', '--tobii_ip', metavar='TOBII_IP', type=str, default=None, help='tobii glasses ip')
parser.add_argument('-c', '--camera_calibration', metavar='CAM_CALIB', type=str, default=None, help='json camera calibration filepath')
parser.add_argument('-p', '--aruco_tracker_configuration', metavar='TRACK_CONFIG', type=str, default=None, help='json aruco tracker configuration filepath')
+ parser.add_argument('-i', '--imu_calibration', metavar='IMU_CALIB', type=str, default=None, help='json imu calibration filepath')
parser.add_argument('-ac', '--aruco_cube', metavar='ARUCO_CUBE', type=str, help='json aruco cube description filepath')
parser.add_argument('-s', '--aoi_scene', metavar='AOI_SCENE', type=str, help='obj aoi 3D scene description filepath')
parser.add_argument('-w', '--window', metavar='DISPLAY', type=bool, default=True, help='enable window display', action=argparse.BooleanOptionalAction)
@@ -103,13 +104,17 @@ def main():
print(f'\nArUcoTracker configuration for markers detection:')
aruco_tracker.print_configuration()
+ # Create tobii imu handler
+ tobii_imu = TobiiInertialMeasureUnit.TobiiInertialMeasureUnit()
+
+ # Load optional imu calibration file
+ if args.imu_calibration != None:
+
+ tobii_imu.load_calibration_file(args.imu_calibration)
+
# Init gyroscope processing to track head rotation changes when arcuco cube pose can't be estimated
# So, the resulting head rotation is relative to last pose estimation (it's not an absolute orientation)
- last_gyroscope = numpy.zeros(3)
- last_gyroscope_ts_ms = 0
- gyroscope_offset = numpy.zeros(3)
- gyroscope_offset_smooth = 0.5
- head_rotation = numpy.zeros(3)
+ tobii_imu.reset_rotation()
# Init accelerotmeter processing to track head translation changes when arcuco cube pose can't be estimated
# So, the resulting head translation is relative to last pose estimation (it's not an absolute position)
@@ -132,6 +137,7 @@ def main():
def data_stream_callback(data_ts, data_object, data_object_type):
+ nonlocal tobii_imu
nonlocal data_ts_ms
data_ts_ms = data_ts / 1e3
@@ -140,38 +146,9 @@ def main():
case 'Gyroscope':
- nonlocal last_gyroscope
- nonlocal last_gyroscope_ts_ms
- nonlocal gyroscope_offset
- nonlocal head_rotation
-
- # Convert deg/s into deg/ms
- current_gyroscope = numpy.array(data_object.value) * 1e-3
-
- # Init gyroscope derivation and integration
- if last_gyroscope_ts_ms == 0:
-
- last_gyroscope = current_gyroscope
- last_gyroscope_ts_ms = data_ts_ms
+ data_object = tobii_imu.apply_gyroscope_offset(data_object)
- # Calculate elapsed time
- delta_time = data_ts_ms - last_gyroscope_ts_ms
-
- # Derivate gyroscope
- gyroscope_derivation = (current_gyroscope - last_gyroscope) / delta_time if delta_time > 0 else numpy.zeros(3)
-
- # Update gyroscope offset smoothly and reset head rotation when gyroscope is stable
- if numpy.linalg.norm(gyroscope_derivation) < 1e-5:
-
- gyroscope_offset = gyroscope_offset * gyroscope_offset_smooth + current_gyroscope * (1 - gyroscope_offset_smooth)
- head_rotation = numpy.zeros(3)
-
- # Integrate gyroscope with offset compensation
- head_rotation += (last_gyroscope - gyroscope_offset) * delta_time
-
- # Store current as last
- last_gyroscope = current_gyroscope
- last_gyroscope_ts_ms = data_ts_ms
+ tobii_imu.update_rotation(data_ts, data_object)
case 'Accelerometer':
@@ -279,7 +256,8 @@ def main():
# Reset head rotation, translation and translation speed (cm/s)
# Note : head translation speed is simultaneously estimated thanks to accelerometer sensor (see upward)
- head_rotation = numpy.zeros(3)
+ tobii_imu.reset_rotation()
+
head_translation = numpy.zeros(3)
head_translation_speed = (tvec - aruco_cube_tvec) / (video_ts_ms - aruco_cube_ts_ms)
@@ -300,13 +278,13 @@ def main():
# Use tobii glasses inertial sensors to estimate cube pose from last estimated pose
# Translate cube according head translation
- new_tvec = aruco_cube_tvec + head_translation
+ new_tvec = aruco_cube_tvec #+ head_translation
#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
- R = make_rotation_matrix(*head_rotation)
+ R = make_rotation_matrix(*tobii_imu.get_rotation())
# rotate tvec ???
#new_tvec = aruco_cube_tvec.dot(R.T)