摄影测量——单片空间后方交会

题目

根据已知条件，摄影机主距f=153.24mm，x0=0.01mm，y0=-0.02mm, 像片比例尺为1:40000，像片上四个像点的框标坐标及其对应的地面坐标，通过单像空间后方交会方法，求解该像片的外方位元素。其中要求各角元素迭代计算至其改正值小于6秒。整个作业通过编程实现。

原理

算法流程

代码实现

import numpy as np
import math as ma
x12138=0
f=153.24e-3
x0=0.01e-3
y0=-0.02e-3
m=40000
phi=kapa=omga=0.0   #角度为弧度制
x=[-86.15e-3,-53.4e-3,-14.78e-3,10.46e-3]
y=[-68.99e-3,82.21e-3,-76.63e-3,64.43e-3]
X=[36589.41,37631.08,39100.97,40426.54]
Y=[25273.32,31324.51,24934.98,30319.81]
Z=[2195.17,728.69,2386.50,757.31]
R=[[ma.cos(phi)*ma.cos(kapa)-ma.sin(phi)*ma.sin(omga)*ma.sin(kapa),
   -ma.cos(phi)*ma.sin(kapa)-ma.sin(phi)*ma.sin(omga)*ma.cos(kapa),
   -ma.sin(phi)*ma.cos(omga)],
   [ma.cos(omga)*ma.sin(kapa),
    ma.cos(omga) * ma.cos(kapa),
    -ma.sin(omga)],
   [
       ma.sin(phi)*ma.cos(kapa)+ma.cos(phi)*ma.sin(omga)*ma.sin(kapa),
       -ma.sin(phi)*ma.sin(kapa)+ma.cos(phi)*ma.sin(omga)*ma.cos(kapa),
       ma.cos(phi)*ma.cos(omga)
   ]   ]
##求定Xs,Ys,Zs的初值
Xs0=Ys0=0.0
Zs0=0.4
for i in X:
    Xs0+=i
Xs0=Xs0/len(X)
for i in Y:
    Ys0+=i
Ys0=Ys0/len(Y)
for i in Z:
    Zs0+=i
Zs0=Zs0/len(Z)
Zs0=m*f+Zs0
A=np.zeros((8,6))
consts=180/ma.pi*3600
dxnp=np.ones((6,1))*7
dxs=np.matrix(dxnp)
num=0
while(abs(dxs[3]*consts)>6 and abs(dxs[4]*consts)>6 and abs(dxs[5]*consts)>6 ):
    _x=np.zeros((4,1))
    _y=np.zeros((4,1))
    for i in range(0,4):
        _x[i]=-f*(R[0][0]*(X[i]-Xs0)+R[1][0]*(Y[i]-Ys0)+R[2][0]*(Z[i]-Zs0))/(R[0][2]*(X[i]-Xs0)+R[1][2]*(Y[i]-Ys0)+R[2][2]*(Z[i]-Zs0))
        _y[i]=-f*(R[0][1] * (X[i] - Xs0) + R[1][1] * (Y[i] - Ys0) + R[2][1] * (Z[i] - Zs0))/ (R[0][2] * (X[i] - Xs0) + R[1][2] * (Y[i] - Ys0) + R[2][2] * (Z[i] - Zs0))
    l=np.zeros((8,1))
    for i in range(0,4):
        l[2*i]=x[i]-_x[i]-x0
        l[2*i+1]=y[i]-_y[i]-y0
    for i in range(0,8,2):
        Zh=R[0][2] * (X[int(i / 2)] - Xs0) + R[1][2] * (Y[int(i / 2)] - Ys0) + R[2][2] * (Z[int(i / 2)] - Zs0)
        A[i][0] = 1 / Zh * (R[0][0] * f + R[0][2] * _x[int(i / 2)])
        A[i][1] = 1 / Zh * (R[1][0] * f + R[1][2] * _x[int(i / 2)])
        A[i][2] = 1 / Zh * (R[2][0] * f + R[2][2] * _x[int(i / 2)])
        A[i][3] = _y[int(i / 2)] * ma.sin(omga) - (_x[int(i / 2)] / f * (_x[int(i / 2)] * ma.cos(kapa) - _y[int(i / 2)] * ma.sin(kapa)) + f * ma.cos(kapa)) * ma.cos(omga)
        A[i][4] = -f * ma.sin(kapa) - _x[int(i / 2)] / f * (_x[int(i / 2)] * ma.sin(kapa) + _y[int(i / 2)] * ma.cos(kapa))
        A[i][5] = _y[int(i / 2)]
    for i in range(1,8,2):
        Zh=R[0][2] * (X[int((i - 1) / 2)] - Xs0) + R[1][2] * (Y[int((i - 1) / 2)] - Ys0) + R[2][2] * (Z[int((i - 1) / 2)] - Zs0)
        A[i][0] = 1 / Zh * (R[0][1] * f + R[0][2] * _y[int((i - 1) / 2)])
        A[i][1] = 1 / Zh * (R[1][1] * f + R[1][2] * _y[int((i - 1) / 2)])
        A[i][2] = 1 / Zh * (R[2][1] * f + R[2][2] * _y[int((i - 1) / 2)])
        A[i][3] = _x[int((i - 1) / 2)] * ma.sin(omga) - (_y[int((i - 1) / 2)] / f * (_x[int((i - 1) / 2)] * ma.cos(kapa) - _y[int((i - 1) / 2)] * ma.sin(kapa)) - f * ma.sin(kapa)) * ma.cos(omga)
        A[i][4] = -f * ma.cos(kapa) - _y[int((i - 1) / 2)] / f * (_x[int((i - 1) / 2)] * ma.sin(kapa) + _y[int((i - 1) / 2)] * ma.cos(kapa))
        A[i][5] = -_x[int((i - 1) / 2)]
    ##利用最小二乘求取变化值
    Am=np.matrix(A)
    dxs=(Am.T*Am).I*(Am.T*l)
    Xs0+=dxs[0]
    Ys0+=dxs[1]
    Zs0+=dxs[2]
    phi+=dxs[3]
    omga+=dxs[4]
    kapa+=dxs[5]
    num+=1
print(omga)
print(kapa)
print(phi)
print(Xs0)
print(Ys0)
print(Zs0)
resultnp=np.ones((6,1))
resultnp[0]=phi
resultnp[1]=omga
resultnp[2]=kapa
resultnp[3]=Xs0
resultnp[4]=Ys0
resultnp[5]=Zs0
np.savetxt('./result.csv',resultnp,fmt='%.18e')
vvsum=Am*dxs-l
vvsum=np.matrix(vvsum)
sigm0=ma.sqrt((vvsum.T*vvsum)/2)
print(sigm0)
qxx=(Am.T*Am).I
print('-****************************-')
print(qxx)
mi=sigm0*np.sqrt(qxx)
print('-----------------------------')
print(mi)
np.savetxt('./mi.csv',mi,fmt='%.18e',delimiter=',')
print(num)

参考文献

[1]. 张剑清等，摄影测量学（第二版） 武汉大学出版社，2015
[2].https://blog.csdn.net/u012176176/article/details/76572430
[3].https://baijiahao.baidu.com/s?id=1617196417868250998&wfr=spider&for=pc
[4].https://blog.csdn.net/xienan_ds_zj/article/details/86738316