HSRW EOLab Wiki

This page is read only. You can view the source, but not change it. Ask your administrator if you think this is wrong.
All the if´s in the end, are not that clean.

<file py findAruco.py>
import numpy as np
import math
import cv2
import cv2.aruco as aruco

cap = cv2.VideoCapture(0)

ARUCO_PARAMETERS = aruco.DetectorParameters_create()
ARUCO_DICT = aruco.Dictionary_get(aruco.DICT_6X6_1000)

screenWidth = int(cap.get(3))
screenHeight = int(cap.get(4))

line1start = (int(round((screenWidth/2))), 0)
line1stop = (int(round((screenWidth/2))), int(round(screenHeight)))
line2start = (0, int(round((screenHeight/2))))
line2stop = (int(round((screenWidth))), int(round((screenHeight/2))))

landingcirclepx = 20

while(True):
        ret, frame = cap.read()
        
        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        
        corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, ARUCO_DICT, parameters=ARUCO_PARAMETERS)
        
        #for corner in corners:
        #    print('Corners: {}'.format(corner))
        
        frame = aruco.drawDetectedMarkers(gray, corners, borderColor=(0,0,255))
        
        frame = cv2.line(frame, line1start, line1stop, (255,255,255), 2)
        frame = cv2.line(frame, line2start, line2stop, (255,255,255), 2)
        frame = cv2.circle(frame, (int(screenWidth*0.5), int(screenHeight*0.5)), landingcirclepx, (255,255,255), 2)
        
        midpoint = (0,0)
        distance = 0
        
        if corners != []:
            midpoint = 0.5*(corners[0][0][0]+corners[0][0][2])
            midpoint = (int(midpoint[0]), int(midpoint[1]))
            frame = cv2.circle(frame, midpoint, 2, (0,0,255), 2)
            distance = math.sqrt((midpoint[0] - (screenWidth / 2))**2 + (midpoint[1] - (screenHeight / 2))**2)
        
        direction = ""

        #V
        if 0 < midpoint[1] and midpoint[1] < (screenHeight / 2 - landingcirclepx):
            direction += "V"
        #H
        if (screenHeight / 2 + landingcirclepx) < midpoint[1] and midpoint[1] < screenHeight:
            direction += "H"
        #R
        if (screenWidth / 2 + landingcirclepx) < midpoint[0] and midpoint[0] < screenWidth:
            direction += "R"
        #L
        if 0 < midpoint[0] and midpoint[0] < (screenWidth / 2 - landingcirclepx):
            direction += "L"
        #Mid
        if (screenWidth / 2 - landingcirclepx) <= midpoint[0] and midpoint[0] <= (screenWidth / 2 + landingcirclepx) and (screenHeight / 2 - landingcirclepx) <= midpoint[1] and midpoint[1] <= (screenHeight / 2 + landingcirclepx):
            direction = "DOWN!"
        
        if direction == "":
            direction = "Nothing found!"
        
        print(direction + " " + str(distance))
        
        cv2.imshow('frame', frame)
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break

cap.release()
cv2.destroyAllWindows()
</file>