165 lines
3.9 KiB
Python
165 lines
3.9 KiB
Python
import math
|
|
import matplotlib.image as mpimg
|
|
import numpy as np
|
|
import matplotlib.pyplot as plt
|
|
import cv2
|
|
import time
|
|
import os
|
|
import pygame
|
|
from PIL import Image
|
|
import random
|
|
os.chdir(os.path.dirname(os.path.abspath(__file__)))
|
|
|
|
|
|
|
|
class getpos():
|
|
def __init__(self):
|
|
# init window
|
|
self.window=pygame.display.set_mode((1600, 720))
|
|
|
|
# coord corner:
|
|
self.coords = []
|
|
|
|
# real table coordinates
|
|
self.origin = [[0,0],[2000,0],[3000,2000],[0,3000]]
|
|
|
|
# pos of robot in wrong coord
|
|
self.pos = np.array([])
|
|
|
|
def main(self):
|
|
self.Done = True
|
|
while self.Done :
|
|
|
|
self.draw()
|
|
self.checkEvent()
|
|
|
|
pygame.display.flip()
|
|
|
|
def draw(self):
|
|
# get image
|
|
self.img = Image.open(('board.jpg'))
|
|
|
|
# resise
|
|
image = self.img.resize((int(1600), int(720)))
|
|
|
|
size = image.size
|
|
mode = image.mode
|
|
data = image.tobytes()
|
|
|
|
|
|
self.displayedSurf = pygame.image.fromstring(data, size, mode)
|
|
self.rect = self.displayedSurf.get_rect()
|
|
self.window.blit(self.displayedSurf, self.rect)
|
|
|
|
def checkEvent(self):
|
|
for event in pygame.event.get():
|
|
|
|
if event.type == pygame.QUIT:
|
|
self.Done = False
|
|
|
|
if event.type == pygame.MOUSEBUTTONDOWN:
|
|
mouse_presses = pygame.mouse.get_pressed()
|
|
if mouse_presses[0]:
|
|
x,y = pygame.mouse.get_pos()
|
|
if len(self.coords) < 4:
|
|
self.coords.append(np.array([x,y]))
|
|
if len(self.coords) == 4:
|
|
self.pos = np.array([x,y])
|
|
|
|
print('\n',self.coords,'\n', self.pos,'\n')
|
|
|
|
X, Y = self.findpos()
|
|
print(1, X, Y)
|
|
|
|
X, Y = self.findpos2()
|
|
print(2, X, Y)
|
|
print(x,y)
|
|
|
|
def unit_vector(self, vector):
|
|
""" Returns the unit vector of the vector. """
|
|
return vector / np.linalg.norm(vector)
|
|
|
|
def angle_between(self, v1, v2):
|
|
|
|
v1_u = self.unit_vector(v1)
|
|
v2_u = self.unit_vector(v2)
|
|
return np.arccos(np.clip(np.dot(v1_u, v2_u), -1.0, 1.0))
|
|
|
|
|
|
|
|
def findpos(self):
|
|
|
|
#
|
|
# A D
|
|
# | E |
|
|
# | / \|
|
|
# B/______C
|
|
#
|
|
|
|
A = self.coords [0]
|
|
B = self.coords [1]
|
|
C = self.coords [2]
|
|
D = self.coords [3]
|
|
E = self.pos
|
|
EBC = self.angle_between(C - B, E - B)
|
|
ECB = self.angle_between(B - C, E - C)
|
|
|
|
ABC = self.angle_between(C-B,A-B)
|
|
BCD = self.angle_between(D-C,B-C)
|
|
|
|
# convert angle to real pos
|
|
EBC *= np.pi/(ABC*2)
|
|
ECB *= np.pi/(BCD*2)
|
|
|
|
# c = T(ECB)/T(EBC) *2000 / 1+ T(ECB)/T(EBC)
|
|
# T(ECB)/T(EBC) = TT
|
|
TT = np.tan(ECB) / np.tan(EBC)
|
|
X = TT*2000/(1+TT)
|
|
Y = np.tan(EBC)*X
|
|
return X, Y
|
|
|
|
def findpos2(self):
|
|
|
|
# _______
|
|
# A D
|
|
# | E |
|
|
# | / \|
|
|
# B/______C
|
|
#
|
|
|
|
A = self.coords [0]
|
|
B = self.coords [1]
|
|
C = self.coords [2]
|
|
D = self.coords [3]
|
|
E = self.pos
|
|
DAE = self.angle_between(D - A, E - A)
|
|
ADE = self.angle_between(E - D, A - D)
|
|
|
|
BAD = self.angle_between(D-A,B-A)
|
|
ADC = self.angle_between(C-D,A-D)
|
|
|
|
# convert angle to real pos
|
|
DAE *= np.pi/(BAD*2)
|
|
ADE *= np.pi/(ADC*2)
|
|
|
|
# c = T(ECB)/T(EBC) *2000 / 1+ T(ECB)/T(EBC)
|
|
# T(ECB)/T(EBC) = TT
|
|
TT = np.tan(ADE) / np.tan(DAE)
|
|
X = TT*2000/(1+TT)
|
|
Y = np.tan(ADE)*X
|
|
return X, Y
|
|
|
|
|
|
|
|
|
|
|
|
|
|
extract = getpos()
|
|
extract.main()
|
|
X, Y = extract.findpos2()
|
|
|
|
print(X, Y)
|
|
|
|
X, Y = extract.findpos()
|
|
|
|
print(X, Y) |