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