HSRW EOLab Wiki

All the if´s in the end, are not that clean.

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()