空间后方交会import numpy as np
from math import*
print('请将数据命名为坐标数据并保存为CSV格式\n')
original_data=np.loadtxt(open('坐标数据.csv'),delimiter=",",skiprows=0)
#读取数据为矩阵形式
print('原始数据如下(x,y,X,Y,Z):\n',original_data)
m=eval(input("请输入比例尺（m）："))
f=eval(input("请输入主距（m）："))
x0,y0=eval(input("请输入x0，y0（以逗号分隔）："))
num=eval(input('请输入迭代次数：'))
xy=[]
XYZ=[]
fi,w,k=0,0,0 #一般相片倾角小于3°所以外方位元素近似取φ，ω，κ=0
#读取影像坐标，存到xy列表，相应地面点坐标存到XYZ列表
for i in range(len(original_data)):
xy.append([original_data[i][0]/1000,original_data[i][1]/1000])
XYZ.append([original_data[i][2],original_data[i][3],original_data[i][4]])
#定义系数矩阵A，常数项矩阵L
A = np.mat(np.zeros((len(xy*2),6)))
L = np.mat(np.zeros((len(xy*2),1)))
#将xy和XYZ列表转化为矩阵
xy=np.mat(xy)
XYZ=np.mat(XYZ)
XYZ_CHA=np.mat(np.zeros((len(xy),3))) #便于推到偏导数建立的矩阵
sum_X=0
sum_Y=0
#Xs0 Ys0 取四个角上控制点坐标的平均值 Zs0=H=mf
for i in range(len(original_data)):
sum_X=original_data[i][2]+sum_X
sum_Y=original_data[i][3]+sum_Y
Xs0=0.25*sum_X
Ys0=0.25*sum_Y
Zs0=m*f
diedai=0
while(diedai<num):
#旋转矩阵
a1=cos(fi)*cos(k)-sin(fi)*sin(w)*sin(k)
a2=(-1.0) * cos(fi) * sin(k) - sin(fi) * sin(w) * cos(k)
a3=(-1.0) * sin(fi) * cos(w)
b1=cos(w) * sin(k)
b2=cos(w) * cos(k)
b3=(-1.0) * sin(w)
c1=sin(fi) * cos(k) + cos(fi) * sin(w) * sin(k)
c2=(-1.0) * sin(fi) * sin(k) + cos(fi) * sin(w) * cos(k)
c3=cos(fi) * cos(w)
xuanzhuan=np.mat([[a1,a2,a3],[b1,b2,b3],[c1,c2,c3]])
for i in range(len(XYZ)):
XYZ_CHA[i,0]=XYZ[i,0]-Xs0
XYZ_CHA[i,1]=XYZ[i,1]-Ys0
XYZ_CHA[i,2]=XYZ[i,2]-Zs0
XYZ_=xuanzhuan.T*XYZ_CHA.T
for i in range(len(XYZ)):
#系数阵：
A[i*2,0]=-f/(Zs0-XYZ[i,2])
A[i*2,1]=0
A[i*2,2]=-xy[i,0]/(Zs0-XYZ[i,2])
A[i*2,3]=-f*(1+pow(xy[i,0],2)/pow(f,2))
A[i*2,4]=-(xy[i,0]*xy[i,1])/f
A[i*2,5]=xy[i,1]
A[i*2+1,0]=0
A[i*2+1,1]=-f/(Zs0-XYZ[i,2])
A[i*2+1,2]=-xy[i,1]/(Zs0-XYZ[i,2])
A[i*2+1,3]=-(xy[i,0]*xy[i,1])/f
A[i*2+1,4]=-f*(1+pow(xy[i,1],2)
扫一扫
2227
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
