hfjh.zip_photogrammetry_外方位元素

%输入初始值 XS0=500215.49; YS0=4185301.89; ZS0=1475.56; phi0=0.054882; omega0=0.057034; kappa0=-0.036175; f=28.1359; x=[-11.00;10.48;10.78;2.712]; y=[-13.80;-6.44;8.312;8.32]; X=[500017.19;500452.19;500470.16;500302.16]; Y=[4185066.00;4185197.50;4185494.50;4185489.00]; Z=[885.40;915.82;928.76;949.71]; delta=10^-6; %显示初始值 disp('像点坐标（mm）：') for i=1:4 fprintf('%f\t',x(i),y(i)) fprintf('\n') end disp('控制点坐标（m）：') for i=1:4 fprintf('%f\t',X(i),Y(i)) fprintf('\n') end disp(['焦距（mm）：',num2str(f)]) disp(['外方位元素初始值：','XS0=',num2str(XS0),'m，YS0=',num2str(YS0),'m，ZS0=',num2str(ZS0),'m，phi0=',num2str(phi0),'rad，omega0=',num2str(omega0),'rad，kappa0=',num2str(kappa0),'rad']) %设置循环条件 n=0; dx=[0.1;0.1;0.1;0.1;0.1;0.1]; while(dx(1)>delta|dx(2)>delta|dx(3)>delta|dx(4)>delta|dx(5)>delta|dx(6)>delta) %计算旋转距阵 n=n+1; a1 = cos(phi0)*cos(kappa0) - sin(phi0)*sin(omega0)*sin(kappa0); a2 = -cos(phi0)*sin(kappa0) - sin(phi0)*sin(omega0)*cos(kappa0); a3 = -sin(phi0)*cos(omega0); b1 = cos(omega0)*sin(kappa0); b2 = cos(omega0)*cos(kappa0); b3 = -sin(omega0); c1 = sin(phi0)*cos(kappa0)+ cos(phi0)*sin(omega0)*sin(kappa0); c2 = -sin(phi0)*sin(kappa0) + cos(phi0)*sin(omega0)*cos(kappa0); c3 = cos(phi0)*cos(omega0); R=[a1 b1 c1;a2 b2 c2;a3 b3 c3]; %计算想点坐标近似值 for i=1:4; appr_x(i,1)=-f*(a1*(X(i,1)-XS0)+b1*(Y(i,1)-YS0)+c1*(Z(i,1)-ZS0))/(a3*(X(i,1)-XS0)+b3*(Y(i,1)-YS0)+c3*(Z(i,1)-ZS0)); appr_y(i,1)=-f*(a2*(X(i,1)-XS0)+b2*(Y(i,1)-YS0)+c2*(Z(i,1)-ZS0))/(a3*(X(i,1)-XS0)+b3*(Y(i,1)-YS0)+c3*(Z(i,1)-ZS0)); end for i=1:4 Xba(i,1)=a1*(X(i,1)-XS0)+b1*(Y(i,1)-YS0)+c1*(Z(i,1)-ZS0); Yba(i,1)=a2*(X(i,1)-XS0)+b2*(Y(i,1)-YS0)+c2*(Z(i,1)-ZS0); Zba(i,1)=a3*(X(i,1)-XS0)+b3*(Y(i,1)-YS0)+c3*(Z(i,1)-ZS0); end %计算误差方程系数及常数项 for i=1:4 a(i*2-1,1)=(a1*f+a3*x(i,1))/Zba(i,1); a(i*2-1,2)=(b1*f+b3*x(i,1))/Zba(i,1); a(i*2-1,3)=(c1*f+c3*x(i,1))/Zba(i,1); a(i*2,1)=(a2*f+a3*y(i,1))/Zba(i,1); a(i*2,2)=(b2*f+b3*y(i,1))/Zba(i,1); a(i*2,3)=(c2*f+c3*y(i,1))/Zba(i,1); a(i*2-1,4)=y(i,1)*sin(omega0)-(x(i,1)*(x(i,1)*cos(kappa0)-y(i,1)*sin(kappa0))/f+f*cos(kappa0))*cos(omega0); a(i*2-1,5)=-f*sin(kappa0)-x(i,1)*(x(i,1)*sin(kappa0)+y(i,1)*cos(kappa0))/f; a(i*2-1,6)=y(i,1); a(i*2,4)=-x(i,1)*sin(omega0)-(y(i,1)*(x(i,1)*cos(kappa0)-y(i,1)*sin(kappa0))/f-f*sin(kappa0))*cos(omega0); a(i*2,5)=-f*cos(kappa0)-y(i,1)*(x(i,1)*sin(kappa0)+y(i,1)*cos(kappa0))/f; a(i*2,6)=-x(i,1); l(i*2-1,1)=x(i,1)-appr_x(i,1); l(i*2,1)=y(i,1)-appr_y(i,1); end %求解外方位元素 A=a; dx=inv(A'*A)*A'*l; V=A*dx-l; XS=XS0+dx(1); YS=YS0+dx(2); ZS=ZS0+dx(3); phi=phi0+dx(4); omega=omega0+dx(5); kappa=kappa0+dx(6); %为再次循环重新赋初始值 XS0=XS; YS0=YS; ZS0=ZS; phi0=phi; omega0=omega; kappa0=kappa; %输出结果 fprintf('\n') disp(['第',num2str(n),'次迭代：']) disp('改正数矩阵为：') for i=1:8 fprintf('%f\t',V(i)) end fprintf('\n') disp('残差阵为：') for i=1:6 fprintf('%f\t',dx(i)) end fprintf('\n') disp(['外方位元素：','XS=',num2str(XS),'m，YS=',num2str(YS),'m，ZS=',num2str(ZS),'m，phi=',num2str(phi),'rad，omega=',num2str(omega),'rad，kappa=',num2str(kappa),'rad']) end