Allowing to draw optic parameters grid.

2 files changed, 27 insertions, 16 deletions

diff --git a/src/argaze/ArUcoMarkers/ArUcoCamera.py b/src/argaze/ArUcoMarkers/ArUcoCamera.py
index 5efc4f8..43a7cf5 100644
--- a/src/argaze/ArUcoMarkers/ArUcoCamera.py
+++ b/src/argaze/ArUcoMarkers/ArUcoCamera.py
@@ -210,11 +210,12 @@ class ArUcoCamera(ArFeatures.ArCamera):
 		# Return dection time and exceptions
 		return detection_time, exceptions
 
-	def __image(self, draw_detected_markers: dict = None, **kwargs) -> numpy.array:
+	def __image(self, draw_detected_markers: dict = None, draw_optic_parameters_grid: dict = None, **kwargs) -> numpy.array:
 		"""Get frame image with ArUco detection visualisation.
 
 		Parameters:
             draw_detected_markers: ArucoMarker.draw parameters (if None, no marker drawn)
+            draw_optic_parameters_grid: OpticParameter.draw parameters (if None, no grid drawn)
             kwargs: ArCamera.image parameters
 		"""
 
@@ -233,6 +234,11 @@ class ArUcoCamera(ArFeatures.ArCamera):
 
 			self.aruco_detector.draw_detected_markers(image, draw_detected_markers)
 
+		# Draw optic parameters grid if required
+		if draw_optic_parameters_grid is not None:
+
+			self.aruco_detector.optic_parameters.draw(image, **draw_optic_parameters_grid)
+
 		# Unlock camera frame exploitation
 		self._frame_lock.release()
 
diff --git a/src/argaze/ArUcoMarkers/ArUcoOpticCalibrator.py b/src/argaze/ArUcoMarkers/ArUcoOpticCalibrator.py
index ec55e44..257b877 100644
--- a/src/argaze/ArUcoMarkers/ArUcoOpticCalibrator.py
+++ b/src/argaze/ArUcoMarkers/ArUcoOpticCalibrator.py
@@ -63,26 +63,31 @@ class OpticParameters():
 
         return output
 
-    def draw(self, image: numpy.array, width:float, height:float, z:float, color=(0, 0, 255)):
+    def draw(self, image: numpy.array, width: float = 0., height:float = 0., z: float = 0., point_size: int = 1, point_color: tuple = (0, 0, 0)):
         """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])
+        if width * height > 0.:
 
-        # 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))
+            # 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])
 
-        # Draw projection
-        for point in grid_2D:
+            # 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))
 
-            # Ignore point out out field
-            try:
-                cv2.circle(image, point.astype(int)[0], 1, color, -1)
-            except:
-                pass
+            # Draw projection
+            for point in grid_2D:
+
+                # Ignore point out out field
+                try:
+
+                    cv2.circle(image, point.astype(int)[0], point_size, point_color, -1)
+
+                except:
+
+                    pass
 
 class ArUcoOpticCalibrator():
     """Handle optic calibration process."""