Properly warning user when pose cannot be validated.

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: