平面坐标系间 同一点(向量)的坐标
平面坐标系间 同一点(向量)的坐标
#!/usr/bin/env python
# coding=utf-8
import numpy as np
import math
# 平面坐标系间 同一点(向量)的坐标
# 已知当前坐标系A下一点Ap,目标坐标系B以及AB的变换关系，求Ap在B下的坐标
# AB坐标系方向是否相同,判断当x方向对齐时y方向是否一致，一致为1 否则-1
f = 1 # A坐标反向 （改变输入和变换）
f2= 1 # B坐标反向（改变输出）
# 设B为固定坐标系，测量A的x轴相对B的x轴之间的夹角，逆时针方向为正。
theta = 3.1415 / 4
# 设A为固定坐标系，要让目标坐标系B的原点与A原点对齐，测量其在A坐标系下位移量
offsetx = 0
offsety = 0
f = np.array([[1, 0], [0, 1 * f]])
offset = np.array([[offsetx], [offsety]])
b = np.array([[1], [0]])
# -----------------------rota
co = np.cos(theta)
si = np.sin(theta)
rota = np.array([[co, -si], [si, co]])
# print (rota)
# -----------------------
b1 = np.dot(f, b)
c1 = np.dot(rota, b1)
result = np.add(offset, c1)
# rota_t=np.linalg.inv(rota)
# print (b1)
# print(c1)
# print(result)
# 变换数据的类，将参数变换为矩阵
class Coordinate2DTransData():
def __init__(self, f=1, f2=1,theta=0, offsetx=0, offsety=0, zoomx=1, zoomy=1):
self.set_transform_info(f, f2,theta, offsetx, offsety, zoomx, zoomy)
#以变换方式设置数据，直接设置让点怎么移动。
def set_transform_info(self,f=1, f2=1,theta=0, offsetx=0, offsety=0, zoomx=1, zoomy=1):
self.f = f #A坐标系y轴需要反向
self.f2 = f2 #B坐标系y轴需要反向
self.theta = theta #B坐标系转theta度x轴与A重合
self.offsetx = offsetx #B与A原点重合 x方向的位移
self.offsety = offsety* f #B与A原点重合 y方向的位移
self.zoomx = zoomx #x缩放
self.zoomy = zoomy #y缩放
# trans A坐标系y轴反向变换矩阵
self.f_mat = np.array([[1, 0], [0, 1 * f]])
# trans B坐标系y轴反向变换矩阵
self.f2_mat = np.array([[1, 0], [0, 1 * f2]])
# offset #B与A原点重合 xy方向的位移
self.offset = np.array([[self.offsetx], [self.offsety]])
self.zoom = np.array([[self.zoomx,0], [0,self.zoomy]])
# rota #角度转弧度
thetapi = math.pi * theta / 180.0
co = np.cos(thetapi)
si = np.sin(thetapi)
#旋转矩阵
self.rota_mat = np.array([[co, -si], [si, co]])
#以坐标系信息方式设置数据，用坐标角位信息变成让点怎么移动。
def set_coordinate_info(self, Af=1, Bf2=1,Btheta=0, Bposix=0, Bposiy=0, Bzoomx=1, Bzoomy=1):
#B在A中的角度 Btheta，旋转变化为 -Btheta,
#B在A中的x Bposix,位移变化为 -Bposix,
#B在A中的y Bposiy,位移变化为 -Bposiy,
self.set_transform_info(Af, Bf2,-Btheta, -Bposix, -Bposiy, Bzoomx, Bzoomy)
# 变换类，输入变换数据后可以做变换
class Coordinate2DTrans():
def __init__(self, td):
self._f = td.f
self._f_mat = td.f_mat
self._offset = td.offset
self._rota_mat = td.rota_mat
self._f2_mat = td.f2_mat
self._zoom =td.zoom
def p2mat(self, x, y):
return np.array([[x], [y]])#
def mat2p(self, mat_p):
# lst=mat_p.tolist()
# return lst[0][0],lst[1][0]
# print type(mat_p.tolist())
return mat_p[0][0], mat_p[1][0]
def trans_f(self, mat_p):
result = np.dot(self._f_mat, mat_p)
return result
def trans_rota(self, mat_p):
result = np.dot(self._rota_mat, mat_p)
return result
def trans(self, mat_p):
result = np.dot(self._f_mat, mat_p)
print("t")
print(result)
result = np.dot(self._rota_mat, result)
print("r")
print(result)
result = np.add(self._offset, result)
print("o")
print(result)
return result
def trans2(self, mat_p):
#输入反转变换
result = np.dot(self._f_mat, mat_p)
#print("t")
#print(result)
# 位移变换
result = np.add(self._offset, result)
#print(self._offset)
#print("o")
#print(result)
# 旋转变换
result = np.dot(self._rota_mat, result)
#print("r")
#print(result)
# 输出放缩变换
result = np.dot(self._zoom, result)
# 输出反转变换
result = np.dot(self._f2_mat, result)
return result
def trans_xy2xy(self, x, y):
return self.mat2p(self.trans(self.p2mat(x, y)))
if __name__ == "__main__":
# 测试1 AB坐标系同向
td = Coordinate2DTransData() # 创建转换数据
td.set_coordinate_info( 1,1, 30, 1.732, 1)
test_ct = Coordinate2DTrans(td)
mat_p = test_ct.p2mat(-1.732, -1)
mat_r = test_ct.trans2(mat_p) # 变换
x1, y1 = test_ct.mat2p(mat_r)
print ("test1")
print (x1,y1)
# 测试2 AB坐标系不同向，B反向
td = Coordinate2DTransData() # 创建转换数据
td.set_coordinate_info( 1,-1, 120, 1.732, 1)
test_ct = Coordinate2DTrans(td)
mat_p = test_ct.p2mat(-1.732, -1)
mat_r = test_ct.trans2(mat_p) # 变换
x1, y1 = test_ct.mat2p(mat_r)
print ("test2")
print (x1,y1)
# 测试2 AB坐标系不同向，A反向
td = Coordinate2DTransData() # 创建转换数据
td.set_coordinate_info( -1,1, -60, 1, 1.732)
test_ct = Coordinate2DTrans(td)
mat_p = test_ct.p2mat(-1,-1.732 )
mat_r = test_ct.trans2(mat_p) # 变换
x1, y1 = test_ct.mat2p(mat_r)
print ("test3")
print (x1,y1)
# 测试2 AB坐标系不同向，A反向
td = Coordinate2DTransData() # 创建转换数据
td.set_coordinate_info( -1,1, -105, 1, 1.732)
test_ct = Coordinate2DTrans(td)
mat_p = test_ct.p2mat(-1,-1.732 )
mat_r = test_ct.trans2(mat_p) # 变换
x1, y1 = test_ct.mat2p(mat_r)
print ("test4")
print (x1,y1)
print("--------------")
# 已知A系的坐标点，求其在B中的坐标
# 设A为固定坐标系，要让B的原点与A原点对齐，测量B在A坐标系下位移量
# 测量B在A坐标系下x轴对齐需要转过的角度，顺时针为正
td = Coordinate2DTransData(1,1, -20, -4, -4)
test_ct = Coordinate2DTrans(td)
mat_p = test_ct.p2mat(6, 6)
mat_r = test_ct.trans2(mat_p) # 先平移后旋转
x1, y1 = test_ct.mat2p(mat_r)
# x1,y1=test_ct.trans_xy2xy(6,6)
print(x1, y1)
print("--------------")
# 已知A系的坐标点，求其在B中的坐标
# 设A为固定坐标系，
# 测量B在A坐标系下位坐标
# 测量B在A坐标系下x轴角度
td = Coordinate2DTransData()
td.set_coordinate_info( 1, 1,20, 4, 4)
test_ct = Coordinate2DTrans(td)
mat_p = test_ct.p2mat(6, 6)
mat_r = test_ct.trans2(mat_p) # 先平移后旋转
x1, y1 = test_ct.mat2p(mat_r)
print(x1, y1)
#td = Coordinate2DTransData(1,1, 20, 4, 4)
#test_ct = Coordinate2DTrans(td)
#x1, y1 = test_ct.trans_xy2xy(2.56, 1.2)
print(x1, y1)
td = Coordinate2DTransData(1,1, -45, -1, -1)
test_ct = Coordinate2DTrans(td)
mat_p = test_ct.p2mat(-1, -1)
mat_r = test_ct.trans2(mat_p) # 先平移后旋转
x1, y1 = test_ct.mat2p(mat_r)
print(x1, y1)
td = Coordinate2DTransData(-1,1, -45, -1, 1)
test_ct = Coordinate2DTrans(td)
mat_p = test_ct.p2mat(-1, 1)
mat_r = test_ct.trans2(mat_p) # 先平移后旋转
x1, y1 = test_ct.mat2p(mat_r)
print(x1, y1)
td = Coordinate2DTransData(1,-1, -45, -1, -1)
test_ct = Coordinate2DTrans(td)
mat_p = test_ct.p2mat(-1, -1)
mat_r = test_ct.trans2(mat_p) # 先平移后旋转
x1, y1 = test_ct.mat2p(mat_r)
print(x1, y1)
print("系统的应用")
#=======================================================================
# 系统的应用
#=======================================================================
# . uwb
# . . N
# . . |
# . . | world
# . . .---------E
# field
# A-----width-----B
# | |
# height |
# | |
# C---------------D
# uwb转world
uwb_world_td = Coordinate2DTransData() # 创建转换数据
uwb_world_td.set_coordinate_info( -1,1, 90, 1.5, 1.2)#坐标系关系
uwb_world_c2dt = Coordinate2DTrans(uwb_world_td) # 创建转换实例
# world 转 field
world_field_td = Coordinate2DTransData() # 创建转换数据
world_field_td.set_coordinate_info( 1,-1, -90, 1.0, 1.3)#坐标系关系
world_field_c2dt = Coordinate2DTrans(world_field_td) # 创建转换实例
# field 转 screen
def screen_select_field_corner(w,h,zx,zy,flag="A"):
field_screen_td = Coordinate2DTransData() # 创建转换数据
# field corner flag
#
# A-----width-----B
# |
# height
# |
# C D
#
# select one corner for screen origin point, for example:
#
# screen show
#
# B-----C height
# |
# |
# |
# width
# |
# |
# A D
#
if flag not in["A","B","C","D" ]:
flag="A"
# 坐标系关系
if flag=="A":
field_screen_td.set_coordinate_info( 1,1,0, 0, 0,screen_zoomx,screen_zoomy)
elif flag=="B":
field_screen_td.set_coordinate_info( 1,1,-90, field_width, 0,screen_zoomx,screen_zoomy)
elif flag=="C":
field_screen_td.set_coordinate_info( 1,1,-180, field_width, field_height, screen_zoomx,screen_zoomy)
elif flag=="D":
field_screen_td.set_coordinate_info( 1,1,-270, 0, field_height, screen_zoomx,screen_zoomy)
# 创建转换实例
return Coordinate2DTrans(field_screen_td)
#场地大小(米)
field_width=8
field_height=4.5
#像素比例尺(像素/米)
screen_zoomx=100
screen_zoomy=100
field_screen_c2dt = screen_select_field_corner(field_width,field_height,screen_zoomx,screen_zoomy,"A") # 创建转换实例
#教学系统uwb或gnss最终会把数据点转换到world下，因此screen的显示只考虑从接收world系下的点就ok
#这里 测试变换：uwb系下一点，经过 world field 最终转到 screen下的示例
def uwb2screen(x,y):
p_uwb =uwb_world_c2dt.p2mat(x,y)
p_world = uwb_world_c2dt.trans2(p_uwb) # 变换
p_field = world_field_c2dt.trans2(p_world)
p_screen = field_screen_c2dt.trans2(p_field)
x1, y1 = field_screen_c2dt.mat2p(p_screen)
print(x1, y1)
lst=[[0,0],[1,1],[8,4.5]]
for e in lst:
uwb2screen(e[0],e[1])
pass