aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/argaze/ArUcoMarkers/ArUcoCube.py6
-rw-r--r--src/argaze/TobiiGlassesPro2/TobiiInertialMeasureUnit.py15
-rw-r--r--src/argaze/utils/tobii_stream_arcube_display.py19
3 files changed, 19 insertions, 21 deletions
diff --git a/src/argaze/ArUcoMarkers/ArUcoCube.py b/src/argaze/ArUcoMarkers/ArUcoCube.py
index 3d889da..00f353a 100644
--- a/src/argaze/ArUcoMarkers/ArUcoCube.py
+++ b/src/argaze/ArUcoMarkers/ArUcoCube.py
@@ -200,7 +200,7 @@ class ArUcoCube():
return rvec, tvec
- def estimate_pose(self, tracked_markers):
+ def estimate_pose(self, tracked_markers) -> Tuple[numpy.array, numpy.array, bool, int]:
"""Estimate cube pose from tracked markers (cf ArUcoTracker.track())
* **Returns:**
@@ -219,7 +219,7 @@ class ArUcoCube():
# Don't try to estimate pose if there is no tracked markers
if len(tracked_markers) == 0:
- return self.get_pose()
+ return self.__translation, self.__rotation, self.__succeded, self.__validity
# Look for faces related to tracked markers
tracked_faces = {}
@@ -331,7 +331,7 @@ class ArUcoCube():
#print('----------------------------------------------------')
- return self.get_pose()
+ return self.__translation, self.__rotation, self.__succeded, self.__validity
@property
def translation(self) -> numpy.array:
diff --git a/src/argaze/TobiiGlassesPro2/TobiiInertialMeasureUnit.py b/src/argaze/TobiiGlassesPro2/TobiiInertialMeasureUnit.py
index e37a1de..a445b45 100644
--- a/src/argaze/TobiiGlassesPro2/TobiiInertialMeasureUnit.py
+++ b/src/argaze/TobiiGlassesPro2/TobiiInertialMeasureUnit.py
@@ -222,15 +222,16 @@ class TobiiInertialMeasureUnit():
# print('no valid head plumb')
@property
- def translation(self) -> Tuple[numpy.array, numpy.array]:
- """Return current translation speed and translation values.
+ def translation(self) -> numpy.array:
+ """Return current translation vector."""
- * **Returns:**
- - translation speed vector
- - translation vector
- """
+ return self.__translation
- return self.__translation_speed, self.__translation
+ @property
+ def translation_speed(self) -> numpy.array:
+ """Return current translation speed vector."""
+
+ return self.__translation_speed
def rotate_plumb(self, rvec):
"""Rotate imu plumb to remove gravity effect in accelerometer data."""
diff --git a/src/argaze/utils/tobii_stream_arcube_display.py b/src/argaze/utils/tobii_stream_arcube_display.py
index 69dc79e..9f4a196 100644
--- a/src/argaze/utils/tobii_stream_arcube_display.py
+++ b/src/argaze/utils/tobii_stream_arcube_display.py
@@ -226,7 +226,7 @@ def main():
aruco_tracker.track(video_frame.matrix)
# Estimate cube pose from tracked markers
- tvec, rvec, success, validity = aruco_cube.estimate_pose(aruco_tracker.get_tracked_markers())
+ tvec, rvec, success, validity = aruco_cube.estimate_pose(aruco_tracker.tracked_markers)
# Cube pose estimation succeed and is validated by 2 faces at least
if success and validity >= 2:
@@ -266,11 +266,10 @@ def main():
imu_tvec = aruco_cube_tvec + numpy.array(TobiiInertialMeasureUnit.CAMERA_TO_IMU_TRANSLATION_VECTOR)
# Translate cube according head translation
- head_translation_speed, head_translation = tobii_imu.get_translation()
- imu_tvec = imu_tvec + head_translation
+ imu_tvec = imu_tvec + tobii_imu.translation
# Rotate cube around imu origin according head rotation
- imu_rvec = tobii_imu.get_rotation() * numpy.array([-1., -1., 1.])
+ imu_rvec = tobii_imu.rotation * numpy.array([-1., -1., 1.])
imu_R = make_rotation_matrix(*imu_rvec)
new_imu_tvec = imu_tvec.dot(imu_R)
@@ -278,7 +277,7 @@ def main():
new_tvec = new_imu_tvec - numpy.array(TobiiInertialMeasureUnit.CAMERA_TO_IMU_TRANSLATION_VECTOR)
# Rotate cube orientation (supposing cube top is up in )
- imu_rvec = tobii_imu.get_rotation() * numpy.array([1., -1., 1.])
+ imu_rvec = tobii_imu.rotation * numpy.array([1., -1., 1.])
imu_R = make_rotation_matrix(*imu_rvec)
C, _ = cv.Rodrigues(aruco_cube_rvec)
@@ -287,20 +286,18 @@ def main():
#new_rvec = aruco_cube_rvec
# Set cube pose estimation
- aruco_cube.set_pose(tvec = new_tvec, rvec = new_rvec)
+ aruco_cube.translation = new_tvec
+ aruco_cube.rotation = new_rvec
else:
raise UserWarning('Cube pose estimation fails.')
- # Get final estimated cube pose
- tvec, rvec, _, _ = aruco_cube.get_pose()
-
# 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(tvec, rvec, aruco_camera.get_K())
+ aoi2D_scene = aoi3D_scene.project(aruco_cube.translation, aruco_cube.rotation, aruco_camera.K)
# Draw projected scene
#aoi2D_scene.draw(visu_frame.matrix)
@@ -309,7 +306,7 @@ def main():
#aruco_tracker.draw_tracked_markers(visu_frame.matrix)
# Draw cube pose estimation (without camera distorsion)
- aruco_cube.draw(visu_frame.matrix, aruco_camera.get_K(), (0, 0, 0, 0), draw_faces=False)
+ aruco_cube.draw(visu_frame.matrix, aruco_camera.K, aruco_camera.D, draw_faces=False)
# Warn about cube pose validity
if not aruco_cube_validity: