摄影测量编程1–空间后方交会
所需的数据需要建一个文件
1)获取已知数据。从航摄资料中查取平均航高与摄影机主距;获取控制点的地面测量坐标并转换为地面摄影测量坐标。
(2)量测控制点的像点坐标并做系统改正。
(3)确定未知数的初始值。在竖直摄影且地面控制点大致分布均匀的情况下,按如下方法确定初始值,即: q=w=k=0式中:m为摄影比例尺分母;n为控制点个数。
(4)用三个角元素的初始值,计算个方向余弦,组成旋转矩阵R。
(5)逐点计算像点坐标的近似值。利用未知数的近似值和控制点的地面坐标代入共线方程式,逐点计算像点坐标的近似值(x)、(y)。
(6)逐点计算误差方程式的系数和常数项,组成误差方程式。
(7)计算法方程的系数矩阵A和常数项L,组成法方程式。
(8)解法方程,求得外方位元素的改正数dXs,dYs,dZs,dq,dw,dk。
(9)用前次迭代取得的近似值,加本次迭代的改正数,计算外方位元素的新值。
(10)将求得的外方位元素改正数与规定的限差比较,若小于限差则迭代结束。否则用新的
近似值重复(4)~(9),直到满足要求为止
在这里插入代码片
import numpy as sy
from math import *
import tkinter as tk
import easygui as g
import tkinter.filedialog
import tkinter.messagebox as tm
from tkinter import messagebox
g.msgbox('打开文件',ok_button = '选择文件')
path = tk.filedialog.askopenfilename()#读取文件路径
test=sy.loadtxt(path,delimiter=',',skiprows=0)#读取文件数据
print(len(test))
print('原始数据如下((x,y),(X,Y,Z)):\n',test)
m=eval(input('请输入比例尺(m):'))
f=eval(input('请输入主距(m):'))
x0,y0=0,0
xy=[]
XYZ=[]
num=0
for i in range(len(test)):
xy.append([test[i][0]/1000,test[i][1]/1000])
#除以1000是因为统一单位
XYZ.append([test[i][2],test[i][3],test[i][4]])
#分别形成像点控制点的矩阵
A=sy.mat(sy.zeros((8,6)))
#四个控制点,每个是2行6列,四个控制点是8行6列
#v=ax-l
#mat函数用来将列表转换成相应矩阵 zeros函数是生成指定维数的全0数组zeros8部分,里面有2个内容
L=sy.mat(sy.zeros((8,1)))
R=sy.mat(sy.zeros((3,3)))
pds=sy.mat(sy.zeros((4,3)))
XS0=0
YS0=0
#旋转矩阵
for i in range(len(test)):
XS0=(test[i][2]+XS0)/4
YS0=(test[i][3]+YS0)/4
ZS0=m*f
print('线元素的初始坐标如下(XS0,YS0,ZS0):\n')
print(XS0)
print(YS0)
print(ZS0)
#Xs0=sumxti/t为初始值
pi=0
w=0
k=0
diedai=0
while(True):
a1 = cos(pi)*cos(k)-sin(pi)*sin(w)*sin(k)
a2 = (-1.0) * cos(pi) * sin(k) - sin(pi) * sin(w) * cos(k)
a3 = (-1.0) * sin(pi) * cos(w)
b1 = cos(w) * sin(k)
b2 = cos(w) * cos(k)
b3 = (-1.0) * sin(w)
c1 = sin(pi) * cos(k) + cos(pi) * sin(w) * sin(k)
c2 = (-1.0) * sin(pi) * sin(k) + cos(pi) * sin(w) * cos(k)
c3 = cos(pi) * cos(w)
R=sy.mat([[a1,a2,a3],[b1,b2,b3],[c1,c2,c3]])
#第四步,计算旋转矩阵RA[i * 2,0] = (-1.0) * f / (ZS0 )
for i in range(0,len(XYZ)):
x = xy[i][0]
y = xy[i][1]
Xp = a1*(XYZ[i][0]-XS0) + b1*(XYZ[i][1]-YS0) + c1*(XYZ[i][2]-ZS0);
Yp = a2*(XYZ[i][0]-XS0) + b2*(XYZ[i][1]-YS0) + c2*(XYZ[i][2]-ZS0);
Zp = a3*(XYZ[i][0]-XS0) + b3*(XYZ[i][1]-YS0) + c3*(XYZ[i][2]-ZS0);
#求平均数
A[2*i, 0] = (a1*f + a3*(x-x0))/Zp
A[2*i, 1] = (b1*f + b3*(x-x0))/Zp
A[2*i, 2] = (c1*f + c3*(x-x0))/Zp
A[2*i, 3] = (y-y0)*sin(w)-(((x-x0)*((x-x0)*cos(k) - (y-y0)*sin(k)))/f+ f*cos(k))*cos(w);
A[2*i, 4] = -f*sin(k) - (x-x0)*((x-x0)*sin(k) +(y-y0))*cos(k)/ f
A[2*i, 5] = y-y0;
A[2*i+1, 0] = (a2*f + a3*(y-y0)) / Zp
A[2 * i + 1, 1] = (b2*f + b3*(x-x0))/ Zp
A[2 * i + 1, 2] = (c2*f + c3*(x-x0))/Zp
A[2 * i + 1, 3] = -(x-x0)*sin(w) - ((y-y0)*(x*cos(k) - y)*sin(k)) / f - f*sin(k)*cos(w);
A[2 * i + 1, 4] = -f*cos(k) - (y-y0)/ f*((x-x0)*sin(k) + (y-y0)*cos(k))
A[2 * i + 1, 5] = -(x-x0)
#a11等系数'''
L[i * 2,0]=x+f*(Xp/Zp)
L[i * 2 + 1,0] =y+f*(Yp/Zp)
#常数项l
Result = [0]*(9+4)
#存改正数的
Result=((A.T*A).I)*A.T*L
XS0+=Result[0]
YS0+=Result[1]
ZS0+=Result[2]
pi+=Result[3]
w+=Result[4]
k+=Result[5]
diedai=diedai+1
if (fabs (Result[3]) < 1e-6) and (fabs (Result[4]) < 1e-6) and (fabs (Result[5]) < 1e-6):
break
if diedai >60:
print("Error:overtime")
break
a1=cos(pi)*cos(k)-sin(pi)*sin(w)*sin(k)
a2=(-1.0) * cos(pi) * sin(k) - sin(pi) * sin(w) * cos(k)
a3=(-1.0) * sin(pi) * cos(w)
b1=cos(w) * sin(k)
b2=cos(w) * cos(k)
b3=(-1.0) * sin(w)
c1=sin(pi) * cos(k) + cos(pi) * sin(w) * sin(k)
c2=(-1.0) * sin(pi) * sin(k) + cos(pi) * sin(w) * cos(k)
c3=cos(pi) * cos(w)
rotate=sy.mat([[a1,a2,a3],[b1,b2,b3],[c1,c2,c3]])
list1=["XS0:",XS0,"YS0:",YS0,"ZS0:",ZS0,"pi:",pi,"w:",w,"k:",k]
print('计算结果\n','外方位元素:\n:'"XS0:",XS0,'\n',"YS0:",YS0,'\n',"ZS0:",ZS0,'\n','PI:',pi,'\n','W:',w,'\n',"K:",k,'\n')
print('旋转矩阵\n',rotate)
print('迭代次数为:',diedai)
messagebox.showinfo("外方位元为:",list1)