1 files changed, 22 insertions, 0 deletions
diff --git a/src/argaze/ArUcoMarkers/ArUcoCamera.py b/src/argaze/ArUcoMarkers/ArUcoCamera.py
index 97436e9..c3c03bd 100644
--- a/src/argaze/ArUcoMarkers/ArUcoCamera.py
+++ b/src/argaze/ArUcoMarkers/ArUcoCamera.py
@@ -6,6 +6,7 @@ from argaze import DataStructures
 
 import json
 import numpy
+import cv2
 import cv2.aruco as aruco
 
 K0 = numpy.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 0.]])
@@ -55,6 +56,27 @@ class CalibrationData():
 
         return output
 
+    def draw(self, frame, width:float, height:float, z:float, color=(0, 0, 255)):
+        """Draw grid to display K and D"""
+
+        # Edit 3D grid
+        grid_3D = []
+        for x in range(-int(width/2), int(width/2)):
+            for y in range(-int(height/2), int(height/2)):
+                grid_3D.append([x, y, z])
+
+        # Project 3d grid
+        grid_2D, _ = cv2.projectPoints(numpy.array(grid_3D).astype(float), numpy.array([0., 0., 0.]), numpy.array([0., 0., 0.]), numpy.array(self.K), -numpy.array(self.D))
+
+        # Draw projection
+        for point in grid_2D:
+
+            # Ignore point out out field
+            try:
+                cv2.circle(frame, point.astype(int)[0], 1, color, -1)
+            except:
+                pass
+
 class ArUcoCamera(CalibrationData):
     """Handle camera calibration process."""