Fixing ArUcoCube utils script.

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: