Added ArUco example

docs/source/lib/Aruco.rst

@@ -0,0 +1,83 @@
+.. _lib_vision:
+
+Aruco
+*******
+
+Examples
+=========
+
+Recognizing ArUco tags
+-------------------------
+
+.. code-block:: python
+
+    import time
+    import cv2
+    from cv2 import aruco
+    from compLib import Vision
+
+    ARUCO_DICT = cv2.aruco.Dictionary_get(aruco.DICT_6X6_250)
+    ARUCO_PARAMETERS = aruco.DetectorParameters_create()
+
+    def getTagCenterFromFrame(id, frame):
+        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
+
+        corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, ARUCO_DICT, parameters = ARUCO_PARAMETERS)
+        frame = aruco.drawDetectedMarkers(frame.copy(), corners, ids)
+
+        if ids is None:
+            return frame, None, None
+
+        for tag_id, corner in zip(ids, corners):
+            if (tag_id[0] == id):
+
+                x, y = 0, 0
+                for i in range(4):
+                    x += corner[0][i][0] * 0.25
+                    y += corner[0][i][1] * 0.25
+
+                return frame, x, y
+        return frame, None, None
+
+    # Get the center from the aruco tag with the specified id
+    # in pixel coordinates (0-640, 0-480)
+    def getTagPosition(id):
+        frame = Vision.Streaming.get_frame()
+        frame, x, y = getTagCenterFromFrame(id, frame)
+        Vision.Streaming.publish_frame(frame)
+        return x, y
+
+    # Get the normalized center coordinates from the aruco tag 
+    # with the specified id 
+    # left is -1, right +1
+    # bottom is -1, top +1
+    def getNormalizedTagPosition(id):
+        frame = Vision.Streaming.get_frame()
+        frame, x, y = getTagCenterFromFrame(id, frame)
+        Vision.Streaming.publish_frame(frame)
+
+        if x is None or y is None:
+            return None, None
+
+        height, width = frame.shape[:2]
+        x = x / width * 2.0 - 1.0
+        y = -(y / height * 2.0 - 1.0)
+        return x, y
+
+    if __name__ == '__main__':
+
+        desiredID = 11
+
+        while True:
+            x, y = getNormalizedTagPosition(desiredID)
+
+            if x is not None:
+                print("X Coordinate: ", x)
+            else:
+                print("Tag not found")
+
+
+This example shows how to recognize ArUco tags based on their id and position.
+You can specify an ID of the tag you want to use and if it's found, the coordinates of the center are returned.
+With the normalized function this is very easy: The x-coordinate is -1 on the left, 1 on the right and 0 in the center of the screen, same for y.
+This way it is quite simple to act on the position of the tag.