单像空间后方交会编程实现

文中有详细注释，根据《摄影测量学（第三版）》（武汉大学出版）进行编写

import numpy as np from numpy import * from math import* from numpy.linalg
import inv print('坐标数据并保存为CSV格式\n') # 读取数据为矩阵形式 original_data1 =
np.loadtxt(open('Coordinat_Date.csv'), delimiter=",", skiprows=0) #
输入计算参数，以数组的形式 print("请依次输入比例尺，主距，x0,y0,迭代次数，均以逗号隔开") A1 = input("
比例尺，主距，x0，y0，迭代次数: ").split() B = list(map(int, A1)) print(B) m = B[0] f1 =
B[1] x01, y01 = B[2], B[3] num1 = B[4] # 建立后方交会的类，用于面向对象编程 class Resection(): #
初始化参数 def __init__(self): self.original_data = original_data1 # 读取数据为矩阵形式
self.f = f1 self.x0, y0 = x01, y01 self.num = num1 self.xy = [] self.XYZ = []
self.fi, self.w, self.k = 0, 0, 0 # 《摄影测量学》P.78
一般相片倾角小于3°所以外方位元素近似取φ，ω，κ=0,书中原话 # 读取影像坐标，存到xy列表，相应地面点坐标存到XYZ列表 self.xuanzhuan
= [] self.Xs0 = [] self.Ys0 = [] self.Zs0 = [] self.rotate = [] self.diedai = 0
# 建立转换方法函数 # 《摄影测量学》P.74 def coordinate(self, m): for i in
range(len(self.original_data)): self.xy.append([self.original_data[i][0]/1000,
self.original_data[i][1]/1000]) self.XYZ.append([self.original_data[i][2],
self.original_data[i][3], self.original_data[i][4]]) # 定义系数矩阵A，常数项矩阵L A =
np.mat(np.zeros((len(self.xy*2), 6))) L = np.mat(np.zeros((len(self.xy*2), 1)))
# 将xy和XYZ列表转化为矩阵 self.xy = np.mat(self.xy) self.XYZ = np.mat(self.XYZ) XYZ_CHA
= np.mat(np.zeros((len(self.xy), 3))) # 便于推到偏导数建立的矩阵 sum_X = 0 sum_Y = 0 # Xs0
Ys0 取四个角上控制点坐标的平均值 Zs0=H=mf，# 《摄影测量学》P.78上方有详细说明 for i in
range(len(self.original_data)): sum_X = self.original_data[i][2]+sum_X sum_Y =
self.original_data[i][3]+sum_Y self.Xs0 = 0.25*sum_X self.Ys0 = 0.25*sum_Y
self.Zs0 = m * self.f while(self.diedai<self.num): #旋转矩阵 a1 =
cos(self.fi)*cos(self.k)-sin(self.fi)*sin(self.w)*sin(self.k) a2 = (-1.0) *
cos(self.fi) * sin(self.k) - sin(self.fi) * sin(self.w) * cos(self.k) a3 =
(-1.0) * sin(self.fi) * cos(self.w) b1 = cos(self.w) * sin(self.k) b2 =
cos(self.w) * cos(self.k) b3 = (-1.0) * sin(self.w) c1 = sin(self.fi) *
cos(self.k) + cos(self.fi) * sin(self.w) * sin(self.k) c2 = (-1.0) *
sin(self.fi) * sin(self.k) + cos(self.fi) * sin(self.w) * cos(self.k) c3 =
cos(self.fi) * cos(self.w) self.xuanzhuan = np.mat([[a1, a2, a3], [b1, b2, b3],
[c1, c2, c3]]) for i in range(len(self.XYZ)): XYZ_CHA[i, 0] = self.XYZ[i,
0]-self.Xs0 XYZ_CHA[i, 1] = self.XYZ[i, 1]-self.Ys0 XYZ_CHA[i, 2] = self.XYZ[i,
2]-self.Zs0 self.XYZ_ = self.xuanzhuan.T*XYZ_CHA.T for i in
range(len(self.XYZ)): # 系数阵： A[i*2, 0] = -self.f/(self.Zs0-self.XYZ[i, 2])
A[i*2, 1] = 0 A[i*2, 2] = -self.xy[i, 0]/(self.Zs0-self.XYZ[i, 2]) A[i*2, 3] =
-self.f*(1+pow(self.xy[i, 0], 2)/pow(self.f, 2)) A[i*2, 4] = -(self.xy[i,
0]*self.xy[i, 1])/self.f A[i*2, 5] = self.xy[i, 1] A[i*2+1, 0] = 0 A[i*2+1, 1]
= -self.f/(self.Zs0 - self.XYZ[i, 2]) A[i*2+1, 2] = -self.xy[i, 1]/(self.Zs0 -
self.XYZ[i, 2]) A[i*2+1, 3] = -(self.xy[i, 0]*self.xy[i, 1])/self.f A[i*2+1, 4]
= -self.f*(1+pow(self.xy[i, 1], 2)/pow(self.f, 2)) A[i*2+1, 5] = -self.xy[i, 0]
# 常数项： L[i * 2, 0] = self.xy[i, 0]+self.f*(self.XYZ_[0, i]/self.XYZ_[2, i]) L[i
* 2 + 1, 0] = self.xy[i, 1]+self.f*(self.XYZ_[1, i]/self.XYZ_[2, i]) # 结果：
Result = ((A.T * A).I)*A.T*L # 《摄影测量学》P.74-5-6 self.Xs0 += Result[0] self.Ys0
+= Result[1] self.Zs0 += Result[2] self.fi += Result[3] self.w += Result[4]
self.k += Result[5] self.diedai = self.diedai+1 # 像点坐标的改正值 V = A * Result - L
sigma0 = sqrt((V.T*V)/(2*4-6)) print("单位权中误差：") print(sigma0) Q = inv(A.T * A)
for i in range(1,6): D = Q[i, i] # 外方位元素的精度 sigma = sqrt(D) * sigma0
print("对角线上外方位元素精度：") print(sigma) a1 =
cos(self.fi)*cos(self.k)-sin(self.fi)*sin(self.w)*sin(self.k) a2 = (-1.0) *
cos(self.fi) * sin(self.k) - sin(self.fi) * sin(self.w) * cos(self.k) a3 =
(-1.0) * sin(self.fi) * cos(self.w) b1 = cos(self.w) * sin(self.k) b2 =
cos(self.w) * cos(self.k) b3 = (-1.0) * sin(self.w) c1 = sin(self.fi) *
cos(self.k) + cos(self.fi) * sin(self.w) * sin(self.k) c2 = (-1.0) *
sin(self.fi) * sin(self.k) + cos(self.fi) * sin(self.w) * cos(self.k) c3 =
cos(self.fi) * cos(self.w) self.rotate = np.mat([[a1, a2, a3], [b1, b2, b3],
[c1, c2, c3]]) # 实例化对象 T = Resection() t = T.coordinate(m) #
以比例尺作为变量代入，这里的变量代入是为了调用函数而已，变量也可以是其他的参数值 print('原始数据如下(x,y,X,Y,Z):\n',
original_data1) print('计算结果\n', T.Xs0, '\n', T.Ys0, '\n', T.Zs0, '\n')
print('旋转矩阵\n', T.rotate) print('迭代次数为：', T.diedai)
程序中的Coordinat_Date.csv格式如下：
-86.15,-68.99,36589.41,25273.32,2195.17 -53.4,82.21,37.63108,31324.51,728.69
-14.78,-76.63,39100.97,24934.98,2396.5 10.46,64.43,40426.54,30319.81,757.31
可以根据自己的需求使用不同的检验代码：表中数据每一行都以x,y,X,Y,Z的顺序排列
坐标数据并保存为CSV格式 请依次输入比例尺，主距，x0,y0,迭代次数，均以逗号隔开 比例尺，主距，x0，y0，迭代次数: 1200 1 1 1 20
[1200, 1, 1, 1, 20] 单位权中误差： 0.03808031015889729 对角线上外方位元素精度： 13.002322081468744
对角线上外方位元素精度： 205.3896722827482 对角线上外方位元素精度： 0.0346622511350548 对角线上外方位元素精度：
0.04088792697435863 对角线上外方位元素精度： 0.5470752369098285 原始数据如下(x,y,X,Y,Z):
[[-8.615000e+01 -6.899000e+01 3.658941e+04 2.527332e+04 2.195170e+03]
[-5.340000e+01 8.221000e+01 3.763108e+01 3.132451e+04 7.286900e+02]
[-1.478000e+01 -7.663000e+01 3.910097e+04 2.493498e+04 2.396500e+03] [
1.046000e+01 6.443000e+01 4.042654e+04 3.031981e+04 7.573100e+02]] 计算结果
[[5814720.85507374]] [[-6723902.61979151]] [[791.63275503]] 旋转矩阵 [[-0.94997338
0.30254713 0.07756161] [-0.06446153 0.05306299 -0.99650842] [-0.30560642
-0.95165621 -0.03090578]] 迭代次数为： 20 Process finished with exit code 0
有不好的地方，还请各位看官指正批评！

• « 上一篇：C#winform在DataGridView控件中加入ComboBox下拉列表框
• » 下一篇：TensorFlow之文本分类算法-3