diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/argaze/utils/tobii_stream_arcube_display.py | 132 |
1 files changed, 79 insertions, 53 deletions
diff --git a/src/argaze/utils/tobii_stream_arcube_display.py b/src/argaze/utils/tobii_stream_arcube_display.py index 09d35f5..bed3ba6 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('-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('-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) args = parser.parse_args() @@ -70,6 +71,14 @@ def main(): aruco_cube = ArUcoCube.ArUcoCube(args.aruco_cube) aruco_cube.print_cache() + # Load AOI 3D scene centered onto aruco cube + aoi3D_scene = AOI3DScene.AOI3DScene() + aoi3D_scene.load(args.aoi_scene) + + print(f'\nAOI in {os.path.basename(args.aoi_scene)} scene related to ArCube:') + for aoi in aoi3D_scene.keys(): + print(f'\t{aoi}') + # Create aruco camera aruco_camera = ArUcoCamera.ArUcoCamera() @@ -94,15 +103,17 @@ def main(): aruco_tracker.print_configuration() # Init head movment estimation - last_accelerometer = numpy.array([]) + ''' + last_accelerometer = numpy.zeros(3) last_accelerometer_ts_ms = 0 - last_gyroscope = numpy.array([]) - last_gyroscope_ts_ms = 0 - gyroscope_drift = numpy.zeros(3) head_translation_speed = numpy.zeros(3) - head_rotation_speed = numpy.zeros(3) gravity = -9.81 - + ''' + last_gyroscope = numpy.zeros(3) + last_gyroscope_ts_ms = 0 + gyroscope_drift = numpy.zeros(3) + head_rotation = numpy.zeros(3) + # Init data timestamped in millisecond data_ts_ms = 0 @@ -112,20 +123,51 @@ def main(): def data_stream_callback(data_ts, data_object, data_object_type): - nonlocal last_accelerometer - nonlocal last_accelerometer_ts_ms - nonlocal last_gyroscope - nonlocal last_gyroscope_ts_ms - nonlocal gyroscope_drift - nonlocal head_translation_speed - nonlocal head_rotation_speed nonlocal data_ts_ms data_ts_ms = data_ts / 1e3 match data_object_type: - case 'Accelerometer': + case 'Gyroscope': + + nonlocal last_gyroscope + nonlocal last_gyroscope_ts_ms + nonlocal gyroscope_drift + nonlocal head_rotation + + # Convert deg/s into deg/ms + current_gyroscope = numpy.array(data_object.value) * 1e-3 + + # Init gyroscope derivation + if last_gyroscope_ts_ms == 0: + + last_gyroscope = current_gyroscope + last_gyroscope_ts_ms = data_ts_ms + + # Derivate gyroscope + delta_time = data_ts_ms - last_gyroscope_ts_ms + gyroscope_derivation = (current_gyroscope - last_gyroscope) / delta_time if delta_time > 0 else numpy.zeros(3) + + # 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 + head_rotation = numpy.zeros(3) + print(f'> gyroscope_drift={gyroscope_drift}') + + # Integrate gyroscope with drift compensation + head_rotation += (last_gyroscope - gyroscope_drift) * delta_time + + # Store current as last + last_gyroscope = current_gyroscope + last_gyroscope_ts_ms = data_ts_ms + + #case 'Accelerometer': + ''' + nonlocal last_accelerometer + nonlocal last_accelerometer_ts_ms + nonlocal head_translation_speed # Convert m/s2 into cm/ms2 current_accelerometer = numpy.array(data_object.value) * 1e-4 @@ -142,32 +184,7 @@ def main(): head_translation_speed += (last_accelerometer + current_accelerometer) * (data_ts_ms - last_accelerometer_ts_ms) / 2 # print(head_translation_speed) - - case 'Gyroscope': - - # convert deg/s into deg/ms - current_gyroscope = numpy.array(data_object.value) * 1e-3 - - # Init gyroscope derivation - if last_gyroscope_ts_ms == 0: - - last_gyroscope = current_gyroscope - last_gyroscope_ts_ms = data_ts_ms - gyroscope_derivation = 0 - - # Derivate gyroscope - else: - gyroscope_derivation = (current_gyroscope - last_gyroscope) / (data_ts_ms - last_gyroscope_ts_ms) - - # Update gyroscope drift when is not moving - if numpy.linalg.norm(gyroscope_derivation) < 1e-7: - - gyroscope_drift = current_gyroscope - - # Drift compensation - head_rotation_speed = current_gyroscope - gyroscope_drift - - #print(head_rotation_speed) + ''' tobii_data_stream.reading_callback = data_stream_callback @@ -213,7 +230,9 @@ def main(): aruco_cube_rvec = rvec aruco_cube_success = success aruco_cube_validity = validity - aruco_cube_ts_ms = video_ts_ms + + # reset head rotation + head_rotation = numpy.zeros(3) # Cube pose estimation fails: use tobii glasses inertial sensors to estimate cube pose from last estimated pose elif aruco_cube_success: @@ -221,25 +240,32 @@ def main(): # Translate cube according head translation speed #aruco_cube_tvec += head_translation_speed * (video_ts_ms - aruco_cube_ts_ms) - #print('after:') - #print(aruco_cube_tvec) - - # Rotate cube around origin according head rotation speed - R = make_rotation_matrix(* (head_rotation_speed * (video_ts_ms - aruco_cube_ts_ms))) - aruco_cube_tvec = aruco_cube_tvec.dot(R.T) - aruco_cube_ts_ms = video_ts_ms + #if numpy.linalg.norm(head_rotation) > 0: + # print(f'X={head_rotation[0]:3f}, Y={head_rotation[1]:3f}, Z={head_rotation[2]:3f}') - #print('berore rotation: ', aruco_cube_tvec) - #print(R) - #print('after rotation: ', aruco_cube_tvec) + # 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) # Set cube pose estimation - aruco_cube.set_pose(tvec = aruco_cube_tvec, rvec = aruco_cube_rvec) + aruco_cube.set_pose(tvec = aruco_cube_tvec, rvec = new_rvec) else: raise UserWarning('Cube pose estimation fails.') + # Project AOI 3 scene onto camera frame + + # DON'T APPLY CAMERA DISTORSION : it projects points which are far from the frame into it + # This hack isn't realistic but as the gaze will mainly focus on centered AOI, where the distorsion is low, it is acceptable. + aoi2D_scene = aoi3D_scene.project(aruco_cube_tvec, aruco_cube_rvec, aruco_camera.get_K()) + + # Draw projected scene + aoi2D_scene.draw(visu_frame.matrix) + # Draw markers pose estimation #aruco_tracker.draw_tracked_markers(visu_frame.matrix) @@ -254,7 +280,7 @@ def main(): # Write warning except UserWarning as w: - cv.rectangle(visu_frame.matrix, (0, 100), (500, 150), (127, 127, 127), -1) + cv.rectangle(visu_frame.matrix, (0, 100), (600, 150), (127, 127, 127), -1) cv.putText(visu_frame.matrix, str(w), (20, 140), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv.LINE_AA) # Assess loop performance |