main.asv

摄影测量学中单像空间后方交会程序

%空间后方交会的解算；

%1、获取已知数据：
%   1.1、读入相片比例尺：blc，内方位元素x0,y0,f;
blc=1/30000;          %相片比例尺
f=0.150;  x0=0;  y0=0 ; %内方位元素
H=f/blc;              %单位为米


%   1.2、获得地面摄影测量坐标X，Y，Z；
X=[100002 100000 105900 105800]';     %单位为米;
Y=[137000 142000 136900 142100]';     %单位为米;
Z=[12 37 41 55]';                     %单位为米;

%2、读入控制点的像点坐标；
x=[-91.345 -94.031 91.890 90.242]'.*10^(-3);   %单位为毫米;
y=[-90.160 65.530 -94.111 70.100]'.*10^(-3);   %单位为毫米;

%3、读入未知数的初始值；
XS0=103000;   %单位为米；
YS0=140000;   %单位为米；
ZS0=4800.00;  %单位为米；
Phi=0;        %弧度单位
Omega = 0;    %弧度单位   
Kappa = 0;    %弧度单位

fid=fopen('Result.txt','a+');

for k=1:20
%循环体部分：
    %4、计算旋转矩阵R；
    RPhi=RP(Phi);        %   4.1、计算RPhi;
    ROmega=RO(Omega);    %   4.2、计算ROmega;
    RKappa=RK(Kappa);       %   4.3、计算RKappa;
    R=RPhi*ROmega*RKappa;%   4.4、计算R。

    %5、逐点计算像点坐标的近似值：利用共线方程计算控制点像点坐标的近似值xs,ys;
    [xs,ys]=wf2xf(R,f,X,Y,Z,XS0,YS0,ZS0);

    %6、组成误差方程：
    A=NorCoe(f,H,x,y);   %   6.1、利用66页5-10式计算误差方程系数A
    lx=x-xs;
    ly=y-ys;               %   6.2、利用62页5-4式计算常数项L;
    L=[lx(1),ly(1),lx(2),ly(2),lx(3),ly(3),lx(4),ly(4)]';

    %7、解误差方程，得到外方位元素新的近似值。
    O=inv(A'*A)*A'*L;
    V=A*O-L;          %单位为毫米

    %8、检查是否收敛;
    length=0.00001;angle=1/180/60*pi;
    K=[abs(O(1))<length,abs(O(2))<length,abs(O(3))<length,abs(O(4))<angle,abs(O(5))<angle,abs(O(6))<angle]';
    %   8.1、收敛则退出循环；
        J=sum(K);
        if J==6
         fprintf(fid,'总共循环了%d次.最终外方位元素改正数为:\r\nV=[%2.9f,%2.9f,%2.9f,%2.9f,%2.9f,%2.9f]\r\n',k,O(1),O(2),O(3),O(4),O(5),O(6))
           XS0=O(1)+XS0;
           YS0=O(2)+YS0;
           ZS0=O(3)+ZS0;
           Phi=O(4)+Phi;
           Omega=O(5)+Omega;
           Kappa=O(6)+Kappa;

         break;          
        else 
           XS0=O(1)+XS0;
           YS0=O(2)+YS0;
           ZS0=O(3)+ZS0;
           Phi=O(4)+Phi;
           Omega=O(5)+Omega;
           Kappa=O(6)+Kappa;
           %xs=xs+V([1,3,5,7]);
           %ys=ys+V([2,4,6,8]);
           fprintf(fid,'第%d次循环的外方位元素改正数:\r\nV=[%2.9f,%2.9f,%2.9f,%2.9f,%2.9f,%2.9f]\r\n',k,O(1),O(2),O(3),O(4),O(5),O(6))
           %fprintf('第%g次循环的外方位元素改正数:\n',k);disp(O);
               
        end
end

%输出结果
%显示结果;
Xs=floor(XS0*100000+0.5)/100000;
Ys=floor(YS0*100000+0.5)/100000;
Zs=floor(ZS0*100000+0.5)/100000;
Phi=r2d(Phi);
Omega=r2d(Omega);
Kappa=r2d(Kappa);
fprintf(fid,'最终外方位元素分别为:\r\nXs=%0.10g米,Ys=%0.10g米,Zs=%0.10g米,Phi=%0.6g,Omega=%0.6g,Kappa=%0.6g\r\n',Xs,Ys,Zs,Phi,Omega,Kappa)



fclose(fid);