Matlab解算空间后方交会外方位元素

---

  本问题为武汉大学摄影测量学教材课后习题，现在用MATLAB实现，供大家学习参考。

1 问题描述

  已知摄像机主距f=153.24mm,四对点的像点坐标与相应的地面坐标列入下表${}^{[1]}$：

点号	像点坐标		地面坐标
	x(mm)	y(mm)	X(mm)	Y(mm)	Z(mm)
1	-86.15	-68.99	36589.41	25273.32	2195.17
2	-53.40	82.21	37631.08	31324.51	728.69
3	-14.78	-76.63	39100.97	24934.98	2386.50
4	10.46	64.43	40426.54	30319.81	757.31

2 实现部分

  直接上代码

%单像空间后方交会%
%编程环境：MATLAB%

%起算数据
x=[-86.15 -53.40 -14.78 10.46];
y=[-68.99 82.21 -76.63 64.43];
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];
f=153.24;
m=50000;%比例尺暂时定为50000

%单位转换，mm2m
x=x/1000;
y=y/1000;
f=f/1000;

%确定外方位元素初始值
XS=mean(X(:));
YS=mean(Y(:));
ZS=m*f;
phi=0;
omg=0;
kai=0;

%设置迭代次数记录函数“dd”
dd=-1;

while(1)  %判断为真就循环

%确定方向余弦 
    a1=cos(phi)*cos(kai)-sin(phi)*sin(omg)*sin(kai);
    a2=-cos(phi)*sin(kai)-sin(phi)*sin(omg)*cos(kai);
    a3=-sin(phi)*cos(omg);
    b1=cos(omg)*sin(kai);
    b2=cos(omg)*cos(kai);
    b3=-sin(omg);
    c1=sin(phi)*cos(kai)+cos(phi)*sin(omg)*sin(kai);
    c2=-sin(phi)*sin(kai)+cos(phi)*sin(omg)*cos(kai);
    c3=cos(phi)*cos(omg);

%构造辅助函数(注意每个辅助函数的值不同)
    for i=1:4
	Xb(i)=a1*(X(i)-XS)+b1*(Y(i)-YS)+c1*(Z(i)-ZS);
	Yb(i)=a2*(X(i)-XS)+b2*(Y(i)-YS)+c2*(Z(i)-ZS);
	Zb(i)=a3*(X(i)-XS)+b3*(Y(i)-YS)+c3*(Z(i)-ZS);
    end

%分别计算4个控制点x，y近似值
    for i=1:4
	xj(i) = -f*Xb(i)/Zb(i);
	yj(i) = -f*Yb(i)/Zb(i);
	L(2*i-1,1) = x(i)-xj(i);
	L(2*i,1) = y(i)-yj(i);
    end

%误差方程式中系数（偏导数）矩阵的计算
    for i=1:4
	A(2*i-1,1)=(a1*f+a3*x(i))/Zb(i);
	A(2*i-1,2)=(b1*f+b3*x(i))/Zb(i);
	A(2*i-1,3)=(c1*f+c3*x(i))/Zb(i);
	A(2*i-1,4)=y(i)*sin(omg)-(x(i)*(x(i)*cos(kai)-y(i)*sin(kai))/f+f*cos(kai))*cos(omg);
	A(2*i-1,5)=-f*sin(kai)-x(i)*(x(i)*sin(kai)+y(i)*cos(kai))/f;
	A(2*i-1,6)=y(i);
	A(2*i,1)=(a2*f+a3*y(i))/Zb(i);
	A(2*i,2)=(b2*f+b3*y(i))/Zb(i);
	A(2*i,3)=(c2*f+c3*y(i))/Zb(i);
	A(2*i,4)=-x(i)*sin(omg)-(y(i)*(x(i)*cos(kai)-y(i)*sin(kai))/f-f*sin(kai))*cos(omg);
	A(2*i,5)=-f*cos(kai)-y(i)*(x(i)*sin(kai)+y(i)*cos(kai))/f;
	A(2*i,6)=-x(i);
    end

%计算法方程
    GZS=inv(A'*A)*A'*L;
    XX=GZS+[XS;YS;ZS;phi;omg;kai];
    XS=XX(1);
    YS=XX(2);
    ZS=XX(3);
    phi=XX(4);
    omg=XX(5);
    kai=XX(6);
    dd=dd+1;

%判断改正数精度（%限差应该是多少？）
    if(abs(GZS(1))<3*(10)^(-5)&&abs(GZS(2))<3*(10)^(-5)&&abs(GZS(3))<3*(10)^(-5)&&abs(GZS(4))<3*(10)^(-5)&& abs(GZS(5))<3*(10)^(-5)&& abs(GZS(6))<3*(10)^(-5))	
	break;
    end  
end

%角度转换
phi=vpa(degrees2dms(rad2deg(XX(4))),4);
omg=vpa(degrees2dms(rad2deg(XX(5))),4);
kai=vpa(degrees2dms(rad2deg(XX(6))),4);
fprintf('迭代次数为%d次\nXS=%.2f\nYS=%.2f\nZS=%.2f\nphi=%d度%d分%.2f秒\nomg=%d度%d分%.2f秒\nkai=%d度%d分%.2f秒',dd,XS,YS,ZS,phi(1),phi(2),phi(3),omg(1),omg(2),omg(3),kai(1),kai(2),kai(3));

计算结果为

参考文献

[1]王佩军, 徐亚明. 摄影测量学:测绘工程专业[M]. 武汉大学出版社, 2005.

 
               赶紧点赞、收藏起来吧！不然下次就找不到了💕