aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--src/argaze/utils/tobii_stream_arcube_display.py23
1 files changed, 17 insertions, 6 deletions
diff --git a/src/argaze/utils/tobii_stream_arcube_display.py b/src/argaze/utils/tobii_stream_arcube_display.py
index 04c47c5..8224d4a 100644
--- a/src/argaze/utils/tobii_stream_arcube_display.py
+++ b/src/argaze/utils/tobii_stream_arcube_display.py
@@ -134,19 +134,30 @@ def main():
# Track markers with pose estimation
aruco_tracker.track(video_frame.matrix)
- # Draw markers pose estimation
- aruco_tracker.draw_tracked_markers(visu_frame.matrix)
-
# Warn user that no markers have been detected
if aruco_tracker.get_tracked_markers_number() == 0:
raise UserWarning('No marker detected')
# Estimate cube pose from tracked markers
- arcube.estimate_pose(aruco_tracker.get_tracked_markers())
+ valid = arcube.estimate_pose(aruco_tracker.get_tracked_markers())
+
+ # Draw markers pose estimation
+ aruco_tracker.draw_tracked_markers(visu_frame.matrix)
+
+ # Draw cube pose estimation
+ arcube.draw(visu_frame.matrix, aruco_camera.get_K(), aruco_camera.get_D())
+
+ # Warn about cube pose validity
+ if not valid:
+
+ raise UserWarning('Cube pose is not validated.')
- # Draw cube axis
- arcube.draw(visu_frame.matrix, aruco_camera.get_K())
+ # Pose estimation error
+ except ValueError as e:
+
+ cv.rectangle(visu_frame.matrix, (0, 100), (500, 150), (127, 127, 127), -1)
+ cv.putText(visu_frame.matrix, str(e), (20, 140), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv.LINE_AA)
# Write warning
except UserWarning as w: