diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/argaze/ArUcoMarkers/ArUcoCube.py | 6 | ||||
| -rw-r--r-- | src/argaze/TobiiGlassesPro2/TobiiInertialMeasureUnit.py | 15 | ||||
| -rw-r--r-- | src/argaze/utils/tobii_stream_arcube_display.py | 19 |
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: |