import numpy as np
import math
class Matrix:
def __init__(self, data):
self.data = np.array(data)
self.row = self.data.shape[0]
self.col = self.data.shape[1] if len(self.data.shape) > 1 else 1
def __getitem__(self, indices):
return self.data[indices]
def __setitem__(self, indices, value):
self.data[indices] = value
def __mul__(self, other):
if isinstance(other, Matrix):
return Matrix(np.dot(self.data, other.data))
elif isinstance(other, (int, float)):
return Matrix(self.data * other)
else:
raise TypeError("Unsupported operand type for *")
def __rmul__(self, other):
return self.__mul__(other)
def T(self):
return Matrix(self.data.T)
@property
def AX(self):
try:
return Matrix(np.linalg.inv(self.data))
except np.linalg.LinAlgError:
# 当矩阵不可逆时,返回伪逆
return Matrix(np.linalg.pinv(self.data))
def __str__(self):
return str(self.data)
def 获取旋转矩阵(fai, omg, kai):
a1 = math.cos(fai) * math.cos(kai) - math.sin(fai) * math.sin(omg) * math.sin(kai)
a2 = -math.cos(fai) * math.sin(kai) - math.sin(fai) * math.sin(omg) * math.cos(kai)
a3 = -math.sin(fai) * math.cos(omg)
b1 = math.cos(omg) * math.sin(kai)
b2 = math.cos(omg) * math.cos(kai)
b3 = -math.sin(omg)
c1 = math.sin(fai) * math.cos(kai) + math.cos(fai) * math.sin(omg) * math.sin(kai)
c2 = -math.sin(fai) * math.sin(kai) + math.cos(fai) * math.sin(omg) * math.cos(kai)
c3 = math.cos(fai) * math.cos(omg)
return Matrix([[a1, a2, a3], [b1, b2, b3], [c1, c2, c3]])
def 后方交会(像点坐标, 地面坐标, f, m=10000):
n = 地面坐标.row
Xs = 0; Ys = 0; Zs = 0
fai = 0; omg = 0; kai = 0
for _ in range(100): # 最大迭代次数设为100
R = 获取旋转矩阵(fai, omg, kai)
a1 = R[0, 0]; a2 = R[0, 1]; a3 = R[0, 2]
b1 = R[1, 0]; b2 = R[1, 1]; b3 = R[1, 2]
c1 = R[2, 0]; c2 = R[2, 1]; c3 = R[2, 2]
A = Matrix(np.zeros((n * 2, 6)))
L = Matrix(np.zeros((n * 2, 1)))
for i in range(n):
x = 像点坐标[i, 0]; y = 像点坐标[i, 1]
X = 地面坐标[i, 0]; Y = 地面坐标[i, 1]; Z = 地面坐标[i, 2]
Xb = a1 * (X - Xs) + b1 * (Y - Ys) + c1 * (Z - Zs)
Yb = a2 * (X - Xs) + b2 * (Y - Ys) + c2 * (Z - Zs)
Zb = a3 * (X - Xs) + b3 * (Y - Ys) + c3 * (Z - Zs)
a11 = (a1 * f + a3 * x) / Zb
a12 = (b1 * f + b3 * x) / Zb
a13 = (c1 * f + c3 * x) / Zb
a21 = (a2 * f + a3 * y) / Zb
a22 = (b2 * f + b3 * y) / Zb
a23 = (c2 * f + c3 * y) / Zb
a14 = y * math.sin(omg) - math.cos(omg) * (x * (x * math.cos(kai) - y * math.sin(kai)) / f + f * math.cos(kai))
a15 = -f * math.sin(kai) - x * (x * math.sin(kai) + y * math.cos(kai)) / f
a16 = y
a24 = -x * math.sin(omg) - (y * (x * math.cos(kai) - y * math.sin(kai)) / f - f * math.sin(kai)) * math.cos(omg)
a25 = -f * math.cos(kai) - y * (x * math.sin(kai) + y * math.cos(kai)) / f
a26 = -x
lx = x + f * Xb / Zb
ly = y + f * Yb / Zb
A[2*i, 0] = a11; A[2*i, 1] = a12; A[2*i, 2] = a13
A[2*i, 3] = a14; A[2*i, 4] = a15; A[2*i, 5] = a16
A[2*i+1, 0] = a21; A[2*i+1, 1] = a22; A[2*i+1, 2] = a23
A[2*i+1, 3] = a24; A[2*i+1, 4] = a25; A[2*i+1, 5] = a26
L[2*i, 0] = lx; L[2*i+1, 0] = ly
dX = (A.T() * A).AX * A.T() * L
Xs += dX[0, 0]
Ys += dX[1, 0]
Zs += dX[2, 0]
fai = (fai + dX[3, 0]) % math.pi
omg = (omg + dX[4, 0]) % math.pi
kai = (kai + dX[5, 0]) % math.pi
if (abs(dX[0, 0]) <= 1.0E-10 and abs(dX[1, 0]) <= 1.0E-10 and
abs(dX[2, 0]) <= 1.0E-10 and abs(dX[3, 0]) <= 1.0E-10 and
abs(dX[4, 0]) <= 1.0E-10 and abs(dX[5, 0]) <= 1.0E-10):
break
return Matrix([[Xs], [Ys], [Zs], [fai], [omg], [kai]])
def 前方交会(左片像点, 右片像点, 左片外方位元素, 右片外方位元素, f):
n = 左片像点.row
地面坐标 = Matrix(np.zeros((n, 3)))
Bx = 右片外方位元素[0, 0] - 左片外方位元素[0, 0]
By = 右片外方位元素[1, 0] - 左片外方位元素[1, 0]
Bz = 右片外方位元素[2, 0] - 左片外方位元素[2, 0]
R左 = 获取旋转矩阵(左片外方位元素[3, 0], 左片外方位元素[4, 0], 左片外方位元素[5, 0])
R右 = 获取旋转矩阵(右片外方位元素[3, 0], 右片外方位元素[4, 0], 右片外方位元素[5, 0])
for i in range(n):
uvw1 = R左 * Matrix([[左片像点[i, 0]], [左片像点[i, 1]], [-f]])
uvw2 = R右 * Matrix([[右片像点[i, 0]], [右片像点[i, 1]], [-f]])
u1 = uvw1[0, 0]; v1 = uvw1[1, 0]; w1 = uvw1[2, 0]
u2 = uvw2[0, 0]; v2 = uvw2[1, 0]; w2 = uvw2[2, 0]
N1 = (Bx * w2 - Bz * u2) / (u1 * w2 - u2 * w1)
N2 = (Bx * w1 - Bz * u1) / (u1 * w2 - u2 * w1)
UVW1 = N1 * uvw1
UVW2 = N2 * uvw2
地面坐标[i, 0] = UVW1[0, 0] + 左片外方位元素[0, 0]
地面坐标[i, 1] = 0.5 * (UVW1[1, 0] + 左片外方位元素[1, 0] + UVW2[1, 0] + 右片外方位元素[1, 0])
地面坐标[i, 2] = UVW1[2, 0] + 左片外方位元素[2, 0]
return 地面坐标
def 后前交会(像点数据, 地面点数据, f):
n = 地面点数据.row
t = 像点数据.row - 地面点数据.row
左片像点 = Matrix(np.zeros((n, 2)))
右片像点 = Matrix(np.zeros((n, 2)))
待求左片像点 = Matrix(np.zeros((t, 2)))
待求右片像点 = Matrix(np.zeros((t, 2)))
for i in range(n):
左片像点[i, 0] = 像点数据[i, 0]
左片像点[i, 1] = 像点数据[i, 1]
右片像点[i, 0] = 像点数据[i, 2]
右片像点[i, 1] = 像点数据[i, 3]
for i in range(t):
待求左片像点[i, 0] = 像点数据[i + n, 0]
待求左片像点[i, 1] = 像点数据[i + n, 1]
待求右片像点[i, 0] = 像点数据[i + n, 2]
待求右片像点[i, 1] = 像点数据[i + n, 3]
左片外方位元素 = 后方交会(左片像点, 地面点数据, f)
右片外方位元素 = 后方交会(右片像点, 地面点数据, f)
待求点地面坐标 = 前方交会(待求左片像点, 待求右片像点, 左片外方位元素, 右片外方位元素, f)
result = "外方位元素测试\\n" + str(左片外方位元素) + "\\n分割线——————————————————\\n" + str(右片外方位元素) + "\\n地面坐标:\\n" + str(待求点地面坐标)
# 保存结果到文件
with open("摄影测量学光束法计算结果.txt", "w", encoding="utf-8") as f:
f.write(result)
print("摄影测量学光束法计算结果已保存到文件: 摄影测量学光束法计算结果.txt")
return result
# 示例数据
像点数据 = Matrix([
[100, 200, 300, 400],
[150, 250, 350, 450],
[200, 300, 400, 500],
[250, 350, 450, 550]
])
地面点数据 = Matrix([
[1000, 2000, 3000],
[1500, 2500, 3500],
[2000, 3000, 4000]
])
f = 100 # 焦距
# 执行后前交会计算
result = 后前交会(像点数据, 地面点数据, f)
print(result)解释代码