110 lines
4.0 KiB
Python
110 lines
4.0 KiB
Python
|
import numpy as np
|
||
|
|
import cv2
|
||
|
|
import math
|
||
|
|
|
||
|
|
|
||
|
|
class CoordinateTransform:
|
||
|
|
def __init__(self,h,w,Radius,foc,pixsize,z):
|
||
|
|
self.w = w
|
||
|
|
self.h = h
|
||
|
|
#a = np.array([[w],[h],[1]])
|
||
|
|
#self.high = z;
|
||
|
|
self.R = Radius #mm
|
||
|
|
#focallengthinpixel=float(foc)/pixsize
|
||
|
|
#self.inverseCalib_matrix = np.array([[float(1)/focallengthinpixel,0,0],[0,float(1)/focallengthinpixel,0],[0,0,1]])
|
||
|
|
#print(a)
|
||
|
|
#b = np.dot(self.inverseCalib_matrix,a)
|
||
|
|
#print(b)
|
||
|
|
#b = b*z
|
||
|
|
#inverse calibration matrix without cx and cy
|
||
|
|
self.xresolution = 156
|
||
|
|
self.yresolution = 96
|
||
|
|
#self.xresolution = b[0][0]
|
||
|
|
#self.yresolution = b[1][0]
|
||
|
|
#print(b)
|
||
|
|
#print(self.xresolution)
|
||
|
|
#print(self.yresolution)
|
||
|
|
|
||
|
|
def transform(self, angle, x, y):
|
||
|
|
angledeg = angle
|
||
|
|
anglerad = float(angle*math.pi)/180
|
||
|
|
beta = (math.pi/2) - anglerad
|
||
|
|
|
||
|
|
Rx = self.R*math.cos(anglerad)
|
||
|
|
Ry = self.R*math.sin(anglerad)
|
||
|
|
|
||
|
|
OldPositioninPixel = np.array([[x],[y],[1]])
|
||
|
|
#print(self.xresolution, self.w,x)
|
||
|
|
m = float(self.xresolution*x)/self.w
|
||
|
|
n = float(self.yresolution*y)/self.h
|
||
|
|
OldPosition = np.array([[m],[n],[1.0]])
|
||
|
|
#print(OldPositioninPixel)
|
||
|
|
#OldPosition = np.dot(self.inverseCalib_matrix,OldPositioninPixel)
|
||
|
|
#print(OldPosition)
|
||
|
|
#OldPosition = OldPosition * self.high
|
||
|
|
#print(OldPosition)
|
||
|
|
|
||
|
|
|
||
|
|
Rotate = np.array([[math.cos(beta),-math.sin(beta),0],[math.sin(beta),math.cos(beta),0],[0,0,1]]) #rotate coordinate
|
||
|
|
Translation1 = np.array([[1,0,Rx],[0,1,-Ry],[0,0,1]]) #transform to camera center
|
||
|
|
|
||
|
|
if angledeg < 90 and angledeg > 0:
|
||
|
|
gamma = (math.pi/2) - anglerad
|
||
|
|
k = (float(self.xresolution)/2) - (float(self.yresolution)/2)*math.tan(gamma)
|
||
|
|
l = (float(self.yresolution)/2)/math.cos(gamma)
|
||
|
|
TranslationQ1 = np.array([[1,0,-(math.cos(gamma)*k)],[0,1,-(math.sin(gamma)*k+l)],[0,0,1]])
|
||
|
|
M1 = np.dot(TranslationQ1,Translation1)
|
||
|
|
#print(TranslationQuadrant1)
|
||
|
|
#print(Translation1)
|
||
|
|
#print(M1)
|
||
|
|
M2 = np.dot(M1,Rotate)
|
||
|
|
#print(M2)
|
||
|
|
M3 = np.dot(M2,OldPosition)
|
||
|
|
print(M3)
|
||
|
|
if angledeg > 90 and angledeg < 180:
|
||
|
|
gamma = (math.pi) - anglerad
|
||
|
|
k = (float(self.xresolution)/2) - (float(self.yresolution)/2)*math.tan(gamma)
|
||
|
|
l = (float(self.yresolution)/2)/math.cos(gamma)
|
||
|
|
TranslationQ2 = np.array([[1,0,-(math.cos(gamma)*k+l)],[0,1,-(math.sin(gamma)*k)],[0,0,1]])
|
||
|
|
M1 = np.dot(TranslationQ2,Translation1)
|
||
|
|
M2 = np.dot(M1,Rotate)
|
||
|
|
M3 = np.dot(M2,OldPosition)
|
||
|
|
if angledeg > 180 and angledeg < 270:
|
||
|
|
gamma = ((3*math.pi)/2) - anglerad
|
||
|
|
k = (float(self.xresolution)/2) - (float(self.yresolution)/2)*math.tan(gamma)
|
||
|
|
l = (float(self.yresolution)/2)/math.cos(gamma)
|
||
|
|
TranslationQ3 = np.array([[1,0,math.cos(gamma)*k],[0,1,math.sin(gamma)*k+l],[0,0,1]])
|
||
|
|
M1 = np.dot(TranslationQ3,Translation1)
|
||
|
|
M2 = np.dot(M1,Rotate)
|
||
|
|
M3 = np.dot(M2,OldPosition)
|
||
|
|
if angledeg > 270 and angledeg < 360:
|
||
|
|
gamma = (2*math.pi) - anglerad
|
||
|
|
k = (float(self.xresolution)/2) - (float(self.yresolution)/2)*math.tan(gamma)
|
||
|
|
l = (float(self.yresolution)/2)/math.cos(gamma)
|
||
|
|
TranslationQ4 = np.array([[1,0,math.sin(gamma)*k+l],[0,1,-(math.cos(gamma)*k)],[0,0,1]])
|
||
|
|
M1 = np.dot(TranslationQ4,Translation1)
|
||
|
|
M2 = np.dot(M1,Rotate)
|
||
|
|
M3 = np.dot(M2,OldPosition)
|
||
|
|
if angledeg == 0:
|
||
|
|
TranslationA0 = np.array([[1,0,-(float(self.xresolution)/2)],[0,1,-(float(self.yresolution)/2)],[0,0,1]])
|
||
|
|
M1 = np.dot(TranslationA0,Translation1)
|
||
|
|
M2 = np.dot(M1,Rotate)
|
||
|
|
M3 = np.dot(M2,OldPosition)
|
||
|
|
if angledeg == 90:
|
||
|
|
TranslationA90= np.array([[1,0,-(float(self.xresolution)/2)],[0,1,-(float(self.yresolution)/2)],[0,0,1]])
|
||
|
|
M1 = np.dot(TranslationA90,Translation1)
|
||
|
|
M2 = np.dot(M1,Rotate)
|
||
|
|
M3 = np.dot(M2,OldPosition)
|
||
|
|
if angledeg == 180:
|
||
|
|
TranslationA180= np.array([[1,0,-(float(self.yresolution)/2)],[0,1,(float(self.xresolution)/2)],[0,0,1]])
|
||
|
|
M1 = np.dot(TranslationA180,Translation1)
|
||
|
|
M2 = np.dot(M1,Rotate)
|
||
|
|
M3 = np.dot(M2,OldPosition)
|
||
|
|
if angledeg == 270:
|
||
|
|
TranslationA270= np.array([[1,0,(float(self.xresolution)/2)],[0,1,(float(self.yresolution)/2)],[0,0,1]])
|
||
|
|
M1 = np.dot(TranslationA270,Translation1)
|
||
|
|
M2 = np.dot(M1,Rotate)
|
||
|
|
M3 = np.dot(M2,OldPosition)
|
||
|
|
# return position with base orientation
|
||
|
|
return M3[0][0], M3[1][0]
|