P30 空间前方交会
信息工程大学：[4.4.1]--空间前方交会_哔哩哔哩_bilibili
另一个参考：《摄影测量学》第09次课 3.2核线几何3.3前方交会1_哔哩哔哩_bilibili
山东农业大学：
2022年9月17号晚上10:35。
前方交会的代码：
https://blog.csdn.net/aaaa202012/article/details/124197239
(注：这个C++代码运行不了。出现的问题是：找不到包含文件：opencv2/opencv2.hpp)
知识点复习：
内方位元素和外方位元素((38条消息) 摄影测量学——航摄像片的内、外方位元素和像点空间直角坐标变换与中心投影构像方程_YuanYWRS的博客-CSDN博客_航摄像片的内定向和外定向元素有哪些)：
外方位元素：摄测与CV 1: 基本坐标系及内参外参 - 知乎 (zhihu.com)
(注：这个知乎上的介绍比较系统)
用摄影测量方法研究被摄物体的几何信息和物理信息，必须建立物体与像片之间的数学关系，因此，首先要确定航空摄影瞬间摄影中心与像片在地面设定的空间坐标系中的位置与姿态。描述这些位置与姿态的参数称为像片的方位元素。
可以通过一个旋转矩阵把像点坐标从像空间坐标系转换到像空间辅助坐标系。这样的话，像点坐标到地面点坐标到坐标转换又近了一步。
像空间坐标系(04大话摄影测量中的五大著名坐标系 - 知乎 (zhihu.com))：
像空间坐标系的原点是主点，即摄影中心在相片上的投影点。
注：
1.像空间坐标系的x轴和y轴不一定平行于像平面坐标系的x轴和y轴。像平面坐标系是固定在相片上的，相片的位置是任意的。
2.像空间辅助坐标系的x轴和y轴平行于地面摄影测量坐标系的坐标轴。
前方交会是利用两张及以上相片的摄影瞬间摄影中心的空间位置和姿态求地物点坐标，后方交会是利用一张相片上至少3个地物点坐标及它们分别对应的像点坐标求摄影瞬间摄影中心的空间位置和姿态。
即：前方交会是根据摄影中心的信息求地物点的信息。
后方交会是根据地物点多信息求摄影中心的信息。
内方位元素，一旦相机制作好了，就定死了。
外方位元素，是一直变化的。
内方位元素：就是相机的摄影中心，或者说是光源中心在框标坐标系中的位置。
外方位元素：就是相机的摄影中心，或者说是光源中心在地面摄影测量坐标系中的位置，和主光轴相对于地面摄影测量坐标系的姿态。
2022/9/19 16:47
之前的C++代码不行，找不到opencv2.hpp,使用matlab代码试试:
(38条消息) 多像空间前方交会（共线条件方程法)_前方交会-其它代码类资源-CSDN文库
matlab代码：
%=========================================% % 摄影测量中， 多张相片的前方交会， %=========================================% clc,clear;%清理命令窗口和工作区间 format long g;%规定输出数据为长整型 format long g;%规定输出数据为长整型 x=[17.383 18.675 19.424 13.981 11.178 11.199;-4.706 -3.428 -2.722 -8.173 -10.947 -10.864]/1000; y=[8.396 4.516 -0.855 -3.870 -0.068 6.970;6.444 2.548 -2.826 -5.793 -1.953 5.081]/1000;%代入左右像片的同名像点坐标 f=0.1014;%主距f f=0.1014;%主距f x0=-0.180/1000;y0=0;%内方位元素 Xs=[50766.27048297550;5140658175310970]; Ys=[85092.60204012240;85098.60787072870]; Zs=[2764.51598579742;2763.02759736577];%代入左右两张像片外方位元素中的线元素值 phi=[0.00749335445825581;-001158295898298480]; omi=[-0.00699425949527119;001003605897533120]; kap=[-0.00213195906904753;0.00849024086417501];%代入左右两张像片外方位元素中的角元素值 B=[]; L=[]; for i=1:2 a1(i)=cos(phi(i))*cos(kap(i))-sin(phi(i))*sin(omi(i))*sin(kap(i)); a2(i)=-cos(phi(i))*sin(kap(i))-sin(phi(i))*sin(omi(i))*cos(kap(i)); a3(i)=-sin(phi(i))*cos(omi(i)); b1(i)=cos(omi(i))*sin(kap(i)); b2(i)=cos(omi(i))*cos(kap(i)); b3(i)=-sin(omi((i))); c1(i)=sin(phi(i))*cos(kap(i))+cos(phi(i))*sin(omi(i))*sin(kap(i)); c2(i)=-sin(phi(i))*sin(kap(i)+cos(phi(i))*sin(omi(i))*cos(kap(i))); c3(i)=cos(phi(i))*cos(omi(i));%计算旋转矩阵R1,R2中各系数 end for j=1:6 for i=1:2 l1(i,j)=f*a1(i)+(x(i,j)-x0)*a3(i); l2(i,j)=f*b1(i)+(x(i,j)-x0)*b3(i); l3(i,j)=f*c1(i)+(x(i,j)-x0)*c3(i); lx(i,j)=f*a1(i)*Xs(i)+f*b1(i)*Ys(i)+f*c1(i)*Zs(i)+(x(i,j)-x0)*a3(i)*Xs(i)+(x(i,j)-x0)*b3(i)*Ys(i)+(x(i,j)-x0)*c3(i)*Zs(i); end end disp('finish!');
python代码：
python实现 空间前方交会 - PythonTechWorld
空间后方交会 python实现(38条消息) 单片空间后方交会 python实现_林林林青冥的博客-CSDN博客_单片空间后方交会
在学校上摄影测量课时，老师给了C的代码来实现后方交会
自己也闲暇时间在学习python，所以自己编程实现空间后方交会求解六个外方位元素（三个线元素，三个角元素）
就用书上的例题得数据，存成了csv格式：
没有设置精度标准，但是可以自己定义迭代次数，需要精度评定的话可以加在循环里。
例题里是四个像点和地面控制点，但写程序得时候没有针对四个点编，如果有更多的点位也是可以的。才疏学浅，应有很多效率更高的法，日后学习了，再多多改进。
题中应用的空间后方交会公式：
共线方程解算外方位元素公式：
进行线性化后：
推导过程：第05讲-共线条件方程线性化 - 百度文库 (baidu.com)
部分截图：
系数阵元素：
像片倾角小于3°，所以外方位角元素近似取为0，近似后如下：
通过公式进行平差计算：
后方交会的Python代码如下：
''' 2022年9月20号凌晨00:21。 摄影测量中的单张相片的后方交会 后方交会的目的是利用1张相片上拍摄的3个地面点大地坐标及对应像点坐标求摄影机摄影中心的6个外方位元素 下面的数据成文件：坐标数据.txt -86.15 -68.99 36589.41 25273.32 2195.17 -53.4 82.21 37631.08 31324.51 728.69 -14.78 -76.63 39100.97 24934.98 2386.5 10.46 64.43 40426.54 30319.81 757.31 ''' import numpy as np from math import* print(r'请将数据命名为坐标数据并保存为CSV格式\n') #original_data=np.loadtxt(open('坐标数据.csv',encoding='gb18030',errors='ignore'),delimiter=",",skiprows=0) #original_data=np.loadtxt(open('坐标数据.csv','rb'),delimiter=",",skiprows=0) original_data=np.loadtxt('坐标数据.txt') #读取数据为矩阵形式 print(r'原始数据如下(x,y,X,Y,Z):\n',original_data) m=eval(input("请输入比例尺（m）：")) f=eval(input("请输入主距（m）：")) x0,y0=eval(input("请输入x0，y0（以逗号分隔）：")) num=eval(input('请输入迭代次数：')) xy=[] XYZ=[] fi,w,k=0,0,0 #一般相片倾角小于3°所以外方位元素近似取φ，ω，κ=0 #读取影像坐标，存到xy列表，相应地面点坐标存到XYZ列表 for i in range(len(original_data)): xy.append([original_data[i][0]/1000,original_data[i][1]/1000]) XYZ.append([original_data[i][2],original_data[i][3],original_data[i][4]]) #定义系数矩阵A，常数项矩阵L A = np.mat(np.zeros((len(xy*2),6))) L = np.mat(np.zeros((len(xy*2),1))) #将xy和XYZ列表转化为矩阵 xy=np.mat(xy) XYZ=np.mat(XYZ) XYZ_CHA=np.mat(np.zeros((len(xy),3))) #便于推到偏导数建立的矩阵 sum_X=0 sum_Y=0 #Xs0 Ys0 取四个角上控制点坐标的平均值 Zs0=H=mf for i in range(len(original_data)): sum_X=original_data[i][2]+sum_X sum_Y=original_data[i][3]+sum_Y Xs0=0.25*sum_X Ys0=0.25*sum_Y Zs0=m*f diedai=0 while(diedai<num): #旋转矩阵 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) xuanzhuan=np.mat([[a1,a2,a3],[b1,b2,b3],[c1,c2,c3]]) for i in range(len(XYZ)): XYZ_CHA[i,0]=XYZ[i,0]-Xs0 XYZ_CHA[i,1]=XYZ[i,1]-Ys0 XYZ_CHA[i,2]=XYZ[i,2]-Zs0 XYZ_=xuanzhuan.T*XYZ_CHA.T for i in range(len(XYZ)): #系数阵： A[i*2,0]=-f/(Zs0-XYZ[i,2]) A[i*2,1]=0 A[i*2,2]=-xy[i,0]/(Zs0-XYZ[i,2]) A[i*2,3]=-f*(1+pow(xy[i,0],2)/pow(f,2)) A[i*2,4]=-(xy[i,0]*xy[i,1])/f A[i*2,5]=xy[i,1] A[i*2+1,0]=0 A[i*2+1,1]=-f/(Zs0-XYZ[i,2]) A[i*2+1,2]=-xy[i,1]/(Zs0-XYZ[i,2]) A[i*2+1,3]=-(xy[i,0]*xy[i,1])/f A[i*2+1,4]=-f*(1+pow(xy[i,1],2)/pow(f,2)) A[i*2+1,5]=-xy[i,0] #常数项： L[i * 2,0]=xy[i,0]+f*(XYZ_[0,i]/XYZ_[2,i]) L[i * 2 + 1,0] =xy[i,1]+f*(XYZ_[1,i]/XYZ_[2,i]) #结果： Result=((A.T*A).I)*A.T*L Xs0+=Result[0] Ys0+=Result[1] Zs0+=Result[2] fi+=Result[3] w+=Result[4] k+=Result[5] diedai=diedai+1 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]]) print(r'计算结果\n',Xs0,'\n',Ys0,'\n',Zs0,'\n') print(r'旋转矩阵\n',rotate) print(r'迭代次数为：',diedai) input()
运行结果：
前方交会的Python代码：
''' 2022年9月19号晚上22:29。 摄影测量中的多张相片的前方交会 ''' 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])
c语言：
(38条消息) 单像空间后方交会（C语言）_南鱼木舟的博客-CSDN博客_单像空间后方交会
1 原理介绍
1.1 定义
空间后方交会 (Space Resection)的定义：利用地面控制点(GCP,Ground Control Point)及其在像片上的像点，确定单幅影像外方位元素的方法。
如果我们知道每幅影像的6个外方位元素，就能确定被摄物体与航摄影像的关系。因此，如何获取影像的外方位元素，一直是摄影测量工作者所探讨的问题。可采取的方法有：利用雷达、全球定位系统（GPS）、惯性导航系统（INS）以及星相摄影机来获取影像的外方位元素；也可利用影像覆盖范围内一定数量的控制点的空间坐标与影像坐标，根据共线条件方程反求该影像的外方位元素，这种方法称为单幅影像的空间后方交会。
1.2 基本思想
以单幅影像为基础，从该影像所覆盖地面范围内的若干控制点的已知地面坐标和相应点的像坐标量测值出发，根据共线条件方程，运用最小二乘间接平差，求解该影像在航空摄影时刻的外方位元素。
由于共线方程是非线性函数，为了运用最小二乘法，必须先将其线性化。
共线条件方程的线性化
某一点的共线方程为：
式中，x,y 为这一点的像平面坐标，x0,y0,f 为影像的内方位元素， Xs,Ys,Zs 为摄站点的物方空间坐标， X,Y,Z 为这一点的物方空间坐标。 ai,bi,ci(i=1,2,3) 为影像旋转矩阵的九个元素，即：
因为未知数是外方位元素，所以将共线方程视为外方位元素的函数。设外方位元素的近似值为
将共线方程在外方位元素近似值处一阶泰勒展开，得：
式中 x0,y0 是把外方位元素近似值代入共线方程中得到的 x,y 。
列出误差方程
将控制点对应的像点的像平面坐标视为观测值，外方位元素视为参数，由共线方程的线性形式可列出误差方程：
式中 x,y 为控制点对应的像点的像平面坐标(相当于是间接平差中的观测值了)，Vx,Vy 为像平面坐标的改正数,
为参数的改正数。
超过3个的地面点坐标相当于是已知数据了。
以上两个方程为一个控制点列出。如果有 n 个控制点，则可以列出 2n 个方程。当 n>=3 时就可求解。
1.3 详细计算
获取已知数据
为了做空间后方交会，需要知道影像比例尺 1/m 、内方位元素 x0,y0,f 、控制点的空间坐标 X,Y,Z ，及其对应像点的像平面坐标 x,y。
影像比例尺可以从摄影资料中查取，也可以利用控制点的空间坐标和其对应像点的像平面坐标进行计算。
确定参数初值
参数的初值即
在竖直航空摄影且地面控制点大体对称分布的情况下，可按如下方法确定初值：
K0 可在航迹图上找出，或根据控制点坐标通过坐标正反变换求出。
计算旋转矩阵
利用角元素近似值计算方向余弦，组成旋转矩阵 R
下面列出三个矩阵相乘的结果供计算
计算像点坐标近似值
利用参数的近似值，按共线方程计算各个控制点对应像点的像平面坐标近似值
计算误差方程系数矩阵和常数项
一个控制点对应的误差方程为
写成矩阵形式为
其中
系数矩阵 A 中的元素均为偏导数。为了计算这些偏导数，引入以下记号：
由于推导过程较为复杂，此处省略，直接给出结果：
对每一个控制点，计算其对应的方程的系数矩阵 Ai 、常数项 Li ，然后联立起来，得：
2.2 问题解读与说明
1．注意单位的换算mm与m。
2．题目给的条件是近似垂直摄影测量，因此初始角度应该设为0，且应采用简化后的公式，不应画蛇添足（这里为了多做练习，采用未进行简化的式子）。
3．初始Z值的设置应为m*f 而不是简单的求平均值（否则会一直不收敛）。由于m值未给出，参照网上代码选取40000。
4．需要迭代计算的只有六个元素，其余变化皆为六个元素经变换等操作求出，迭代结束限差应先考虑角度，再是Z，最后是XY。
5．注意：这里由于共线方程式进行了简化，将x表示x-x0（这样求出来是零！不知道为什么！因此在求解时根据共线方程式再一步转化成了-fX/Z和-fY/Z），计算误差公式的A矩阵求解所用的x并不是像点坐标。
6．求逆矩阵采用代数余子式法，利用递归方法，时间复杂度较高，不能用于大数据的计算，LUV分解法相对省时间，但是我不太会= =。动态申请二维数组可以定义长度不定的数组空间，但是定义较为麻烦。
2.3 c语言求解实现代码
/* --------------------------------------------------------------------- 空间后方交会代码 时间：2019年11月1日 步骤：1.先设置初值 2.再用间接平差法求得改正值 3.用改正值加上初始值进行迭代操作，满足限差要求时退出 4.最后进行精度评价 参考网址：http://www.docin.com/p-641460791.html 矩阵求逆 https://cloud.tencent.com/developer/article/1359605 -------------------------------------------------------------------- */ #include <stdio.h> #include <stdlib.h> #include <math.h> #define GCPNUMBER 4 //设置初始值控制点数量 #define N 6 int main() { int i, j, k; int repeatNumber = 0; //迭代次数 int m = 40000; //比例尺 double x0=0,y0=0,f = 0.15324; //内方位元素 double dOuterElem[6] = { 1,1,1,1,1,1 }; double outerElement[6] = { 0.0 }; //要求的参数:Xs,Ys,Zs,fai,omiga,kaba; //输入控制点空间坐标和像坐标xy和XYZ double GCPLocationxy [GCPNUMBER][2] = { //控制点像坐标 { -0.08615, -0.06899 }, { -0.05340, 0.08221 }, { -0.01478, -0.07663 }, { 0.01046, 0.06443 } }; double GCPLocationXYZ [GCPNUMBER][3] = { //控制点物空间坐标 { 36589.41, 25273.32, 2195.17 }, { 37631.08, 31324.51, 728.69 }, { 39100.97, 24934.98, 2386.50 }, { 40426.54, 30319.81, 757.31 } }; printf("单像空间后方交会：\n\n"); printf("已知条件：\n\n"); printf(" 比例尺：\n m = %d\n\n", m); printf(" 内方位元素：\n x0 = %lf y0 = %lf f = %lf\n\n", x0, y0, f); printf(" 控制点像坐标：\n"); for (i = 0; i < GCPNUMBER; i++) { printf(" "); for (j = 0; j < 2; j++) { printf("%lf ", GCPLocationxy[i][j]); } printf("\n"); } printf("\n 控制点物空间坐标：\n"); for (i = 0; i < GCPNUMBER; i++) { printf(" "); for (j = 0; j < 3; j++) { printf("%lf ", GCPLocationXYZ[i][j]); } printf("\n"); } printf("\n迭代过程："); //给外方位元素赋初值 double sumXYZ[3] = { 0.0 }; for (i = 0; i < 3; i++) { for (j = 0; j < GCPNUMBER; j++) { sumXYZ[i] += GCPLocationXYZ[j][i]; } for (j = 0; j < 2;j++) outerElement[i] = sumXYZ[i] / GCPNUMBER; //XY为四个控制点取平均值 outerElement[i] = m*f + sumXYZ[i] / GCPNUMBER; //Z为m*f } bool GetMatrixInverse(double **src, int n, double des[N][N]); //求逆矩阵声明 do //迭代过程 { double R[3][3] = { 0.0 }; //旋转矩阵R R[0][0] = cos(outerElement[3])*cos(outerElement[5]) - sin(outerElement[3])*sin(outerElement[4])*sin(outerElement[5]); R[0][1] = -cos(outerElement[3])*sin(outerElement[5]) - sin(outerElement[3]) * sin(outerElement[4])*cos(outerElement[5]); R[0][2] = -sin(outerElement[3])*cos(outerElement[4]); R[1][0] = cos(outerElement[4])*sin(outerElement[5]); R[1][1] = cos(outerElement[4])*cos(outerElement[5]); R[1][2] = -sin(outerElement[4]); R[2][0] = sin(outerElement[3])*cos(outerElement[5]) + cos(outerElement[3])*sin(outerElement[4])*sin(outerElement[5]); R[2][1] = -sin(outerElement[3])*sin(outerElement[5]) + cos(outerElement[3])*sin(outerElement[4])*cos(outerElement[5]); R[2][2] = cos(outerElement[3])*cos(outerElement[4]); double Xgang[GCPNUMBER]; for (i = 0; i < GCPNUMBER; i++) Xgang[i] = R[0][0] * (GCPLocationXYZ[i][0] - outerElement[0]) + R[1][0] * (GCPLocationXYZ[i][1] - outerElement[1]) + R[2][0] * (GCPLocationXYZ[i][2] - outerElement[2]); double Ygang[GCPNUMBER]; for (i = 0; i < GCPNUMBER; i++) Ygang[i] = R[0][1] * (GCPLocationXYZ[i][0] - outerElement[0]) + R[1][1] * (GCPLocationXYZ[i][1] - outerElement[1]) + R[2][1] * (GCPLocationXYZ[i][2] - outerElement[2]); double Zgang[GCPNUMBER]; for (i = 0; i < GCPNUMBER; i++) Zgang[i] = R[0][2] * (GCPLocationXYZ[i][0] - outerElement[0]) + R[1][2] * (GCPLocationXYZ[i][1] - outerElement[1]) + R[2][2] * (GCPLocationXYZ[i][2] - outerElement[2]); //xy作为初始值，由共线方程式子求得 double approxxy[GCPNUMBER][2] = { 0.0 }; for (i = 0; i < GCPNUMBER; i++) { approxxy[i][0] = -f*Xgang[i] / Zgang[i]; approxxy[i][1] = -f*Ygang[i] / Zgang[i]; //用下面的数据求结果等于零，不知道为什么 //approxxy[i][0] = GCPLocationxy[i][0]-x0; //approxxy[i][1] = GCPLocationxy[i][1] - y0; } //误差方程式元素A double A[GCPNUMBER * 2][6]; for (i = 0; i < GCPNUMBER; i++) { A[i * 2][0] = (R[0][0] * f + R[0][2] * approxxy[i][0]) / Zgang[i]; A[i * 2][1] = (R[1][0] * f + R[1][2] * approxxy[i][0]) / Zgang[i]; A[i * 2][2] = (R[2][0] * f + R[2][2] * approxxy[i][0]) / Zgang[i]; A[i * 2][3] = (approxxy[i][0] * approxxy[i][1])*R[1][0] / f - (f + pow(approxxy[i][0], 2) / f)*R[1][1] - approxxy[i][1] * R[1][2]; A[i * 2][4] = -pow(approxxy[i][0], 2)*sin(outerElement[5]) / f - (approxxy[i][0] * approxxy[i][1]) / f*cos(outerElement[5]) - f*sin(outerElement[5]); A[i * 2][5] = approxxy[i][1]; A[i * 2 + 1][0] = (R[0][1] * f + R[0][2] * approxxy[i][1]) / Zgang[i]; A[i * 2 + 1][1] = (R[1][1] * f + R[1][2] * approxxy[i][1]) / Zgang[i]; A[i * 2 + 1][2] = (R[2][1] * f + R[2][2] * approxxy[i][1]) / Zgang[i]; A[i * 2 + 1][3] = -(approxxy[i][0] * approxxy[i][1])*R[1][1] / f + (f + pow(approxxy[i][1], 2) / f)*R[1][0] - approxxy[i][0] * R[1][2]; A[i * 2 + 1][4] = -pow(approxxy[i][1], 2)*cos(outerElement[5]) / f - (approxxy[i][0] * approxxy[i][1]) / f*cos(outerElement[5]) - f*sin(outerElement[5]); A[i * 2 + 1][5] = -approxxy[i][0]; } //误差方程式元素L double L[GCPNUMBER * 2]; for (i = 0; i < GCPNUMBER * 2; i++) { if (i % 2 == 0) //x L[i] = GCPLocationxy[i / 2][0] - approxxy[i / 2][0]; else L[i] = GCPLocationxy[i / 2][1] - approxxy[i / 2][1]; } //求转置 double AT[6][GCPNUMBER * 2]; for (i = 0; i < 6; i++) { for (j = 0; j < GCPNUMBER * 2; j++) { AT[i][j] = A[j][i]; } } //求Naa double **Naa=new double*[6]; //----------------------------动态申请数组----------------------------- for(i=0;i<6;i++) { Naa[i]=new double[6]; } for (int i = 0; i < 6; i++) { for (int j = 0; j < 6; j++) { Naa[i][j] = 0.0; } }; //-------------------------上面步骤的可以用 double Naa[6][6]={0.0}; 等效替代----------------- for (i = 0; i < 6; i++) { for (j = 0; j < 6; j++) { for (k = 0; k < GCPNUMBER * 2; k++) { Naa[i][j] += AT[i][k] * A[k][j]; //计算Naa } } } //求Naa逆矩阵 double NaaInv[6][6] = { 0.0 }; bool inverse = GetMatrixInverse(Naa, N, NaaInv); //求ATL double temp[6] = { 0.0 }; for (i = 0; i < 6; i++) //行遍历 { for (j = 0; j < GCPNUMBER * 2; j++) { temp[i] += AT[i][j] * L[j]; } } //求三角号 for (i = 0; i < 6; i++) { dOuterElem[i] = 0.0; for (j = 0; j < 6; j++) { dOuterElem[i] += NaaInv[i][j] * temp[j]; } } //求得迭代后的改正值及迭代后的外方位元素 printf("\n\n 第%d次迭代改正值：\n ", ++repeatNumber); for (i = 0; i < 6; i++) { printf("%lf ", dOuterElem[i]); outerElement[i] = outerElement[i] + dOuterElem[i]; } printf("\n 迭代后外方位元素值：\n "); for (i = 0; i < 6; i++) { printf("%lf ", outerElement[i]); } }while (dOuterElem[3] >= 2.908882087e-5 || dOuterElem[4] >= 2.908882087e-5 || dOuterElem[5] >= 2.908882087e-5);//各角元素迭代计算至其改正值小于6秒,6s=2.908882087e-5 弧度。 //精度评价 printf("\n\n\n精度评价矩阵：\n"); //double sum[6] = { 0.0 }; double outElemAccuracy[6][6] = { 0.0 }; for (j = 0; j < 6; j++) { printf(" "); for (i = 0; i < 6; i++) { outElemAccuracy[j][i] = sqrt(abs(dOuterElem[j] * dOuterElem[i]) / double(2 * GCPNUMBER - 6)); printf("%lf ", outElemAccuracy[j][i]); } printf("\n"); } system( "pause" ); } /* 以下为求逆矩阵函数： 方法：代数余子式乘行列式倒数法 代数余子式用迭代方法完成，行列式也用代数余子式 按第一行展开计算|A| 输入要求逆的矩阵和矩阵行列数 */ double getA(double **arcs , int n) { int i,j,k; if (n == 1) return arcs[0][0]; double ans = 0; //double temp[N][N] = { 0.0 }; double **temp=new double *[n-1]; for(i=0;i<n-1;i++) { temp[i]=new double[n-1]; } for (int i = 0; i < n-1; i++) { for (int j = 0; j < n-1; j++) { temp[i][j]=0.0; } } for (i = 0; i < n; i++) //第一行进行遍历 { //其他行列组成矩阵，求代数余子式 for (j = 0; j < n - 1; j++) { for (k = 0; k < n - 1; k++) { temp[j][k] = arcs[j + 1][(k >= i) ? k + 1 : k]; //printf("%lf ",temp[j][k]); } } //printf("暂时的------"); double t = getA(temp, n - 1); if (i % 2 == 0) { ans += arcs[0][i] * t; //结果：t是代数余子式，arcs【0】【i】是第一行的数字 } else { ans -= arcs[0][i] * t; } } return ans; } //计算每一行每一列的每一个元素所对应的余子式，组成A* void getAStart(double **arcs, int n, double ans[N][N]) { if (n == 1) //如果是一阶行列式 { ans[0][0] = 1; return; } int i, j, k, t; //double temp[N][N]; double **temp=new double*[N]; for (i = 0; i<n - 1; i++) { temp[i] = new double[n - 1]; } for (int i = 0; i < n - 1; i++) { for (int j = 0; j < n - 1; j++) { temp[i][j] = 0.0; } } for (i = 0; i < n; i++) { for (j = 0; j < n; j++) { //每一个矩阵元素的代数余子式 for (k = 0; k < n - 1; k++) { for (t = 0; t < n - 1; t++) { temp[k][t] = arcs[k >= i ? k + 1 : k][t >= j ? t + 1 : t]; } } ans[j][i] = getA(temp, n - 1); if ((i + j) % 2 == 1) { ans[j][i] = -ans[j][i]; } } } delete temp; } //得到给定矩阵src的逆矩阵保存到des中 bool GetMatrixInverse(double **src, int n, double des[N][N]) { double flag = getA(src, n); //|A| double t[N][N]; if (0 == flag) { printf( "原矩阵行列式为0，无法求逆。请重新运行" ); return false; } else { getAStart(src, n, t); //求代数余子式 //求逆矩阵 for (int i = 0; i < n; i++) { for (int j = 0; j < n; j++) { des[i][j] = t[i][j] / flag; } } } return true; }
2.4 程序评价
优点：代码注释全面，输出结果清晰明了，对读代码者较为友好；对于不同的控制点数量只需要更改GCPNUMBER变量即可，无需改变内部函数；用动态申请数组，可以节省部分空间。
缺点：未进行函数的包装，将其过程一股脑扔到了main函数中，显得没有条理；过多使用for循环和数组，感觉程序很臃肿；未进行相似函数的封装（譬如说矩阵乘法）为了省事就直接计算了。
前方交会：
多像空间前方交会的抗差总体最小二乘估计 (chinasmp.com)----测绘学报上的论文
(38条消息) 立体像对空间前方交会算法_超人琪的博客-CSDN博客_空间前方交会
立体像对空间前方交会的C语言代码：
//前方交会 #include<stdio.h> #include<iostream> #include<opencv2/opencv.hpp> #include<math.h> using namespace std; using namespace cv; int main() { //定义同名像点的坐标 double image[2][2] = { 0.0 }; double x1 = 29.214000; double y1 = -47.640000; double x2 = 3.936000; double y2 = -47.910000; x1 = image[0][0]; y1 = image[0][1]; x1 = image[1][0]; y1 = image[1][1]; //定义内方位元素 double x0 = 0.0; double y0 = 0.0; double f = 70.50; //定义外方位元素 double W_ele[2][6] = { {432438.565, 3894841.049, 1274.012 , -0.005785603 , 0.0003784083 , -0.01880947 }, { 432847.537 , 3894836.718 , 1272.414 , -0.006604103 , 0.0005394704 , 0.0007619388} }; double XS1 = W_ele[0][0]; double YS1 = W_ele[0][1]; double ZS1 = W_ele[0][2]; double phi1 = W_ele[0][3]; double omig1 = W_ele[0][4]; double kappa1 = W_ele[0][5]; double XS2 = W_ele[1][0]; double YS2 = W_ele[1][1]; double ZS2 = W_ele[1][2]; double phi2 = W_ele[1][3]; double omig2 = W_ele[1][4]; double kappa2 = W_ele[1][5]; //计算基线分量 double Bx = XS2 - XS1; double By = YS2 - YS1; double Bz = ZS2 - ZS1; //cout << kappa1 << endl; //确定外方位角元素 double a1 = cos(phi1)*cos(kappa1) - sin(phi1)*sin(omig1)*sin(kappa1); double a2= -1.0*cos(phi1)*sin(kappa1) - sin(phi1)*sin(omig1)*cos(kappa1); double a3= -1.0*sin(phi1)*cos(omig1); double b1= cos(omig1)*sin(kappa1); double b2= cos(omig1)*cos(kappa1); double b3 = -1.0*sin(omig1); double c1 = sin(phi1)*cos(kappa1) + cos(phi1)*sin(omig1)*sin(kappa1); double c2 = -1.0*sin(phi1)*sin(kappa1) + cos(phi1)*sin(omig1)*cos(kappa1); double c3 = cos(phi1)*cos(omig1); //定义旋转矩阵R1 Mat R1(3, 3, CV_64F,0.0); R1.at<double>(0, 0) = a1; R1.at<double>(0, 1) = a2; R1.at<double>(0, 2) = a3; R1.at<double>(1, 0) = b1; R1.at<double>(1, 1) = b2; R1.at<double>(1, 2) = b3; R1.at<double>(2, 0) = c1; R1.at<double>(2, 1) = c2; R1.at<double>(2, 2) = c3; double aa1 = cos(phi2)*cos(kappa1) - sin(phi2)*sin(omig2)*sin(kappa2); double aa2 = -1.0*cos(phi2)*sin(kappa1) - sin(phi2)*sin(omig2)*cos(kappa2); double aa3 = -1.0*sin(phi2)*cos(omig1); double bb1 = cos(omig2)*sin(kappa1); double bb2 = cos(omig2)*cos(kappa1); double bb3 = -1.0*sin(omig2); double cc1 = sin(phi2)*cos(kappa2) + cos(phi2)*sin(omig2)*sin(kappa2); double cc2 = -1.0*sin(phi2)*sin(kappa2) + cos(phi2)*sin(omig2)*cos(kappa2); double cc3 = cos(phi2)*cos(omig2); //定义旋转矩阵R2 Mat R2(3, 3, CV_64F, 0.0); R2.at<double>(0, 0) = aa1; R2.at<double>(0, 1) = aa2; R2.at<double>(0, 2) = aa3; R2.at<double>(1, 0) = bb1; R2.at<double>(1, 1) = bb2; R2.at<double>(1, 2) = bb3; R2.at<double>(2, 0) = cc1; R2.at<double>(2, 1) = cc2; R2.at<double>(2, 2) = cc3; //printf("a1=%lf", aa1); //定义同名像点的像空间辅助坐标系RR1(X1，Y1，Z1)和RR2(X2，Y2，Z) Mat RR1(3, 1, CV_64F, 0.0); Mat RR2(3, 1, CV_64F, 0.0); Mat RR11(3, 1, CV_64F, 0.0); RR11.at<double>(0, 0) = x1; RR11.at<double>(1, 0) = y1; RR11.at<double>(2, 0) = -f; Mat RR22(3, 1, CV_64F, 0.0); RR22.at<double>(0, 0) = x2; RR22.at<double>(1, 0) = y2; RR22.at<double>(2, 0) = -f; RR1 = R1*RR11; double X1 = RR1.at<double>(0, 0); double Y1 = RR1.at<double>(1, 0); double Z1 = RR1.at<double>(2, 0); RR2 = R2*RR22; double X2 = RR2.at<double>(0, 0); double Y2 = RR2.at<double>(1, 0); double Z2 = RR2.at<double>(2, 0); cout << "左像空间辅助坐标系：" << endl; cout << X1 << endl; cout << Y1 << endl; cout << Z1 << endl; cout << "右像空间辅助坐标系：" << endl; cout << X2 << endl; cout << Y2 << endl; cout << Z2 << endl; //计算点投影系数 double N1 = (Bx*Z2 - Bz*X2) / (X1*Z2 - Z1*X2); double N2 = (Bx*Z1 - Bz*X1) / (X1*Z2 - Z1*X2); //计算模型点坐标 double deteX = N1*X1; double deteY = 0.5*(N1*Y1 + N2*Y2 + By); double deteZ = N1*Z1; cout << "模型点坐标：" << endl; cout << deteX << endl; cout << deteY << endl; cout << deteZ << endl; //计算地面点的地面坐标（X，Y，Z） double XP = XS1 + deteX; double YP = YS1 + deteY; double ZP = ZS1 + deteZ; cout << "地面点坐标是：" << endl; cout <<"XP="<< XP << endl; cout <<"YP="<< YP<< endl; cout << "ZP="<<ZP<< endl; //std::cout << RR1 << std::endl; getchar(); return 0; } //运行结果 左像空间辅助坐标系： -0.407883 0.0266778 -70.4988 右像空间辅助坐标系： 2.56837 -47.9375 -70.5505 模型点坐标： 56.0753 3289.33 9692.11 地面点坐标是： XP=432495 YP=3.89813e+06 ZP=10966.1
U1,V1是怎样的到的呢？
答：A向W1S1V1平面投影得到的。
N1和N2求出来之后，带入到第2个式子中，假如等号两边的值相差不大的话，就可以求一个平均值，作为待求地面点到Y值。
