0
$\begingroup$

So I have a pan-tilt system with an airbrush on top, the pressure is quite strong so that I can place the robot at a distance of at least 1.5 meters. I currently have normalized coordinates XY that I can visualize on my camera like this

enter image description here

Now I want to translate those coordinates to a real canvas and allow the pan-tilt to point towards them and eventually spray. The two servos have 0 to 180 degrees but the airbrush is positioned on top of the tilt at 90. So if we consider that the pan and tilt it's at 90 the airbrush points perpendicularly to the real canvas. I am following along with this answer https://stackoverflow.com/a/44269392/13475623

lx = (2 * canvasXpoint / canvasW - 1) * np.tan(fovX / 2)
ly = (-2 * canvasYpoint / canvasH + 1) * np.tan(fovY / 2) 
lz = 100

tx = np.cos(pan) * np.cos(tilt) * lx - np.cos(tilt) * np.sin(pan) * ly - np.sin(tilt) * lz
ty = np.sin(pan) * lx + np.cos(pan) * ly
tz = np.cos(pan) * np.sin(tilt) * lx - np.sin(pan) * np.sin(tilt) * ly + np.cos(tilt) * lz

tilt = abs(np.arctan2(tz, tx) )*180 /np.pi
pan  = abs(np.arcsin(ty / np.sqrt(tx*tx + ty*ty + tz*tz))) *180 /np.pi

he specifically asks to use fovx and fovy, but I have no idea how to place the, is fovx and fovy the same as the centre of the canvas plus z? which gives the robot position?

this is the entire code:

import numpy as np
import random
import cv2

rect = (0,0,0,0)
startPoint = False
endPoint = False

def on_mouse(event,x,y,flags,params):

    global rect,startPoint,endPoint

    # get mouse click
    if event == cv2.EVENT_LBUTTONDOWN:

        if startPoint == True and endPoint == True:
            startPoint = False
            endPoint = False
            rect = (0, 0, 0, 0)

        if startPoint == False:
            rect = (x, y, 0, 0)
            startPoint = True
        elif endPoint == False:
            rect = (rect[0], rect[1], x, y)
            endPoint = True

cap = cv2.VideoCapture(0)
waitTime = 50

#Reading the first frame
(grabbed, frame) = cap.read()

# create a numpy array with coordinates, dtype= np.uint32
points = np.array([
    [0.3791454386035252, 0.5089704263689607], [0.4983802415059109, 0.4865878212776629], [0.4191061040406586, 0.4890729258496474], [0.48898375092596835, 0.6904554156787046], [0.41117320428962, 0.6855686449973655], [0.48969027909831686, 0.8806483247709954], [0.4096722346480175, 0.8725103831012889], [0.45146556567120294, 0.216198952126905], [0.6304876750748412, 0.1994776546413951], [0.6406976694235704, 0.1861724655606558], [0.6199918357274865, 0.18561325370105788], [0.6525936779272056, 0.201758477474465], [0.6013198509477334, 0.20041966221830415], [0.6683290543094758, 0.29699362669473495], [0.5645238852104717, 0.3113999818240313], [0.6545654774178274, 0.49620430200480303], [0.5898070573107588, 0.49659117464889346], [0.6592482998457356, 0.6834740545963035], [0.5840631897032319, 0.6828527784533074], [0.6408640096147972, 0.8299668209407426], [0.5829181988101784, 0.8173392725052692], [0.6197806290284397, 0.30050890733295843], [0.8252923243905792, 0.23409826375167195], [0.835683753646597, 0.2185883280832016], [0.8131540844750428, 0.21904862499113367], [0.8506741192799976, 0.2279991219170517], [0.7959142481709739, 0.22725381616179272], [0.8733570624656342, 0.3256920048853457], [0.7652207837892534, 0.3239122878098148], [0.893097550288673, 0.44273291363944955], [0.7346131146711571, 0.4430594635999311], [0.902709244982588, 0.5343829401117663], [0.8520378940615836, 0.543215423861057], [0.7842126810888624, 0.5430821914771806], [0.8496391467917583, 0.7170072127563635], [0.7934480818135997, 0.7157067918591926], [0.8415470663986131, 0.8790693270711738], [0.7969306654944098, 0.8786970205344115], [0.8191112469834433, 0.32444646417244244], [0.4544294400182521, 0.10802826838116084], [0.4652589441860643, 0.09470838455219986], [0.44184697991125976, 0.09401847354478254], [0.4784184639521475, 0.1113126386155105], [0.42270482157448985, 0.10977393520172159], [0.5101597581790689, 0.21719483055184013], [0.39370939342390643, 0.21645334444157344], [0.3703281257159549, 0.34746637604116004]], np.float64)

while(cap.isOpened()):

    (grabbed, frame) = cap.read()

    cv2.namedWindow('frame')
    cv2.setMouseCallback('frame', on_mouse)    
    panarr=[]
    tiltarr=[]

    #drawing rectangle

    if startPoint == True:
        cv2.circle(frame, (rect[0], rect[1]), 2,(255, 0, 255), 2)
    if startPoint == True and endPoint == True:
        
        cv2.rectangle(frame, (rect[0], rect[1]), (rect[2], rect[3]), (255, 0, 255), 2)
        
        w = rect[2] - rect[0]
        h = rect[3] - rect[1]

        canvasW = 120
        canvasH = 90
        distanceZ = 100
        
        #position machine    
        screenXCenter = (rect[0] + rect[2]) / 2
        screenYCenter = (rect[1] + rect[3]) / 2

        pan = tilt = 90

        servoXcentrepoint = canvasW / 2 
        servoYcentrepoint = canvasH / 2 

        # fov
        fovX =  np.arctan((canvasW * canvasH )/distanceZ)
        
        for x, y in points:
           
            screenX = (x * w) + rect[0] #- differencesqrx
            screenY = (y * h) + rect[1] #- differencesqry

            cv2.circle(frame,(int(screenXCenter),int(screenYCenter)),2,(255, 255, 0),2)    
            cv2.circle(frame,(int(screenX),int(screenY)),2,(255, 45, 250),2)


            canvasXpoint = (x * canvasW)
            canvasYpoint = (y * canvasH)

            # dx =  canvasXpoint - servoXcentrepoint 

            # dy =  canvasYpoint - servoYcentrepoint

            # pan = abs(np.arctan((distanceZ/dx))) * 180/np.pi
            # tilt = abs(np.arctan(distanceZ/dy)) * 180/np.pi

            lx = (2 * canvasXpoint / canvasW - 1) * np.tan(servoXcentrepoint / 2)
            ly = (-2 * canvasYpoint / canvasH + 1) * np.tan(servoYcentrepoint / 2) 
            lz = 10

            tx = np.cos(pan) * np.cos(tilt) * lx - np.cos(tilt) * np.sin(pan) * ly - np.sin(tilt) * lz
            ty = np.sin(pan) * lx + np.cos(pan) * ly
            tz = np.cos(pan) * np.sin(tilt) * lx - np.sin(pan) * np.sin(tilt) * ly + np.cos(tilt) * lz

            tilt = abs(np.arctan2(tz, tx) )*180 /np.pi
            pan  = abs(np.arcsin(ty / np.sqrt(tx*tx + ty*ty + tz*tz))) *180 /np.pi

            

          
            
            tiltarr.append(int(tilt))
            panarr.append(int(pan))
            
   

            # a = [x,y]
    
    cv2.imshow('frame',frame)
    if cv2.waitKey(1)==ord('q'):
        break
print(tiltarr)
print(panarr)

cap.release()
cv2.destroyAllWindows()

The ultimate goal is to determine the angle for the pan and tilt based on each point

$\endgroup$

0

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.