python实现 空间前方交会

去年学习摄影测量学的时候，写了python的空间后方交会，想着有时间也写写前方交会，但一直没落实，今年疫情在家，开了一门数字摄影测量课程，再次重温了前方交会，在家也闲来无事，写写前方交会代码

python 空间后方交会程序
https://blog.csdn.net/weixin_44946842/article/details/102662112

这次用到的数据外方位元素已经用上述程序计算好了

计算过程：

参考文献：
[1]杨可明. 摄影测量学基础[M]. 北京：中国电力出版社,2011

简言之就是左右两张像片分别用空间后方交会算出各自的外方位元素，共12个，之后利用内外方位元素和像点坐标计算对应地面点坐标。

数据如下：

---

第一步：

---

先利用空间后方交会算出左右像片的外方位元素
python空间后方交会程序在此！！
迭代次数为均为5

左像片运算结果：

右像片运算结果：

---

第二步

---

进行前方交会

代码如下：

import numpy as np
from math import*
#内方位元素：f，x0，y0  单位mm
In_ele=np.array([[150,0,0]])

#左右像片外方位元素  单位mm
Lout_ele=np.array([[4999757.49582,4999738.04354,1999998.07144]])
Rout_ele=np.array([[5896855.86538,5070303.27142,2030428.07609]])
#手动输入同名像点坐标
L_point=np.array([[51.758,80.555],
                  [14.618,-0.231],
                  [49.88,-0.782],
                  [86.14,-1.346],
                  [48.035,-79.962]])
R_point=np.array([[-39.953,78.463],
                  [-76.006,0.036],
                  [-42.201,-1.022],
                  [-7.706,-2.112],
                  [-44.438,-79.736]])

#旋转矩阵自己输，或者用下边的旋转矩阵函数算也可以
R1=np.array([[0.99546692,-0.09510813,-0.00022301],
              [0.09506152,0.99504727,-0.02905583],
              [0.00298535,0.02890291,0.99957777]])
R2=np.array([[0.99372738,-0.11089881,-0.01439957],
             [0.11013507,0.99285291,-0.04597144],
             [0.01939484,0.04409718,0.99883897]])

def rotate(out_ele):
    #计算旋转矩阵函数 out_ele为外方位角元素的行矩阵[[fi],[w],[k]]
    fi,w,k=out_ele[0,0],out_ele[0,1],out_ele[0,2]
    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)
    rotate=np.mat([[a1,a2,a3],[b1,b2,b3],[c1,c2,c3]])
    return rotate

def BaseLine(L,R):
    #计算基线分量，L.R为左右像片线元素
    B=[]
    for i in range(3):
        B.append(R[0,i]-L[0,i])
    return np.array([B])

def coordinate(R,P,f):#计算所求点的像空间辅助坐标系,xyz--> XYZ,R为旋转矩阵,P所求点像空间坐标,f为主距

    XYZ=[]
    if len(P)>=1:
        for i in range(len(P)):
            
            xyz=np.array([[P[i,0]],[P[i,1]],[(-1)*f]])
            XYZ.append(np.dot(R,xyz))
    return XYZ

def projection_index(B,XYZ1,XYZ2):#投影系数计算
    N1=((B[0,0]*XYZ2[2,0])-(B[0,2]*XYZ2[0,0]))/((XYZ1[0,0]*XYZ2[2,0])-(XYZ2[0,0]*XYZ1[2,0]))
    N2=((B[0,0]*XYZ1[2,0])-(B[0,2]*XYZ1[0,0]))/((XYZ1[0,0]*XYZ2[2,0])-(XYZ2[0,0]*XYZ1[2,0]))
    return [N1,N2]

def GP(XYZ_s1,N,XYZ1):#地面控制点坐标计算
    XA=XYZ_s1[0,0]+N[0]*XYZ1[0]
    YA=XYZ_s1[0,1]+N[0]*XYZ1[1]
    ZA=XYZ_s1[0,2]+N[0]*XYZ1[2]
    return XA/1000,YA/1000,ZA/1000 #换个单位，变成m

L_rotate=R1
R_rotate=R2

B=BaseLine(Lout_ele,Rout_ele)
XYZ1=coordinate(L_rotate,L_point,In_ele[0][0])  #左片像空间辅助坐标
XYZ2=coordinate(R_rotate,R_point,In_ele[0][0])  #右片像空间辅助坐标

N=[]
G_P=[]
for i in range(len(XYZ1)):
    N.append(projection_index(B,XYZ1[i],XYZ2[i]))
for i in range(len(XYZ1)):
    G_P.append(GP(Lout_ele,N[i],XYZ1[i]))
Ground_Point=np.array(G_P)
for i in range(len(G_P)):
    print(str(i+1)+"号点的地面坐标：XA,YA,ZA\n")
    print(Ground_Point[i])

运行结果：

编程是可以更好的理解知识点，同时也能练习练习自己的编程能力，有不足，忘读者见谅！
！！感谢阅读！！
希望疫情早点结束，中国加油！