摄影测量前方交会后方交会matlab源程序

f=0.150;faiz=0;omiz=0;kaz=0;Xdz=[5083.205,5780.02,5210.879,5909.264];Ydz=[5852.099,5906.365,4258.446,4314.283];Zdz=[527.925,571.549,461.81,455.484];%以上为输入gcp1到gcp4的地面坐标xxz=[0.016012,0.08856,0.013362,0.08224];yxz=[0.079963,0.081134,-0.07937,-0.080027];%右片像点坐标Xsz=(5083.205+5780.02+5210.879+5909.264)/4;Ysz=(5852.099+5906.365+4258.446+4314.283)/4;Sxz=sqrt((xxz(2)-xxz(1))^2+(yxz(2)-yxz(1))^2);Sdz=sqrt((Xdz(2)-Xdz(1))^2+(Ydz(2)-Ydz(1))^2);mz=10000Zsz=mz*f+1/4*(Zdz(1)+Zdz(2)+Zdz(3)+Zdz(4));Xz=[1;1;1;1;1;1];while1a1z=cos(faiz)*cos(kaz)-sin(faiz)*sin(omiz)*sin(kaz);a2z=-cos(faiz)*sin(kaz)-sin(faiz)*sin(omiz)*cos(kaz);a3z=-sin(faiz)*cos(omiz);b1z=cos(omiz)*sin(kaz);b2z=cos(omiz)*cos(kaz);b3z=-sin(omiz);c1z=sin(faiz)*cos(kaz)+cos(faiz)*sin(omiz)*sin(kaz);c2z=-sin(faiz)*sin(kaz)+cos(faiz)*sin(omiz)*cos(kaz);c3z=cos(faiz)*cos(omiz);R=[a1z,b1z,c1z;a2z,b2z,c2z;a3z,b3z,c3z]forn=1:1:4s1z=[Xdz(n)-Xsz,Ydz(n)-Ysz,Zdz(n)-Zsz]';X_z(n)=[a1z,b1z,c1z]*s1z;Y_z(n)=[a2z,b2z,c2z]*s1z;Z_z(n)=[a3z,b3z,c3z]*s1z;xz(n)=-f*X_z(n)/Z_z(n);yz(n)=-f*Y_z(n)/Z_z(n);end%以上循环用于求解像空间坐标x(1),y(1)到x(4),y(4)forp=1:1:4a11z(p)=1/Z_z(p)*(a1z*f+a3z*xz(p));a12z(p)=1/Z_z(p)*(b1z*f+b3z*xz(p));a13z(p)=1/Z_z(p)*(c1z*f+c3z*xz(p));a21z(p)=1/Z_z(p)*(a2z*f+a3z*yz(p));a22z(p)=1/Z_z(p)*(b2z*f+b3z*yz(p));a23z(p)=1/Z_z(p)*(c2z*f+c3z*yz(p));a14z(p)=yz(p)*sin(omiz)-(xz(p)/f*(xz(p)*cos(kaz)-yz(p)*sin(kaz))+f*cos(kaz))*cos(omiz);a15z(p)=-f*sin(kaz)-xz(p)/f*(xxz(p)*sin(kaz)+yz(p)*cos(kaz));%计算A矩阵a16z(p)=yxz(p);a24z(p)=-xxz(p)*sin(omiz)-(yz(p)/f*(xz(p)*cos(kaz)-yz(p)*sin(kaz))-f*sin(kaz))*cos(omiz);a25z(p)=-f*cos(kaz)-yz(p)/f*(xz(p)*sin(kaz)+yz(p)*cos(kaz));a26z(p)=-xxz(p);%右片endAz=[a11z(1),a12z(1),a13z(1),a14z(1),a15z(1),a16z(1);a21z(1),a22z(1),a23z(1),a24z(1),a25z(1),a26z(1);a11z(2),a12z(2),a13z(2),a14z(2),a15z(2),a16z(2);a21z(2),a22z(2),a23z(2),a24z(2),a25z(2),a26z(2);a11z(3),a12z(3),a13z(3),a14z(3),a15z(3),a16z(3);a21z(3),a22z(3),a23z(3),a24z(3),a25z(3),a26z(3);a11z(4),a12z(4),a13z(4),a14z(4),a15z(4),a16z(4);a21z(4),a22z(4),a23z(4),a24z(4),a25z(4),a26z(4)]lxz=xxz-xz;lyz=yxz-yz;%右片Lz=[lxz(1),lyz(1),lxz(2),lyz(2),lxz(3),lyz(3),lxz(4),lyz(4)]'Xz=inv(Az'*Az)*Az'*Lzdxz=Xz(1,1);dyz=Xz(2,1);dzz=Xz(3,1);dfz=Xz(4,1);doz=Xz(5,1);dkz=Xz(6,1);Xsz=Xsz+dxz;Ysz=dyz+Ysz;Zsz=dzz+Zsz;faiz=faiz+dfz;omiz=omiz+doz;kaz=kaz+dkz;ifall(abs(Xz)0.0003)break;endendf=0.152;fai=0;omi=0;ka=0;Xd=[5083.205,5780.02,5210.879,5909.264];Yd=[5852.099,5906.365,4258.446,4314.283];Zd=[527.925,571.549,461.81,455.484];%以上为输入gcp1到gcp4的地面坐标xx=[-0.07393,-0.005252,-0.079122,-0.009887];yx=[0.078706,0.078184,-0.078879,-0.080089];%右片像点坐标Xs=(5083.205+5780.02+5210.879+5909.264)/4;Ys=(5852.099+5906.365+4258.446+4314.283)/4;Sx=sqrt((xx(2)-xx(1))^2+(yx(2)-yx(1))^2);Sd=sqrt((Xd(2)-Xd(1))^2+(Yd(2)-Yd(1))^2);m=10000Zs=m*f+1/4*(Zd(1)+Zd(2)+Zd(3)+Zd(4));X=[1;1;1;1;1;1];while1a1=cos(fai)*cos(ka)-sin(fai)*sin(omi)*sin(ka);a2=-cos(fai)*sin(ka)-sin(fai)*sin(omi)*cos(ka);a3=-sin(fai)*cos(omi);b1=cos(omi)*sin(ka);b2=cos(omi)*cos(ka);b3=-sin(omi);c1=sin(fai)*cos(ka)+cos(fai)*sin(omi)*sin(ka);c2=-sin(fai)*sin(ka)+cos(fai)*sin(omi)*cos(ka);c3=cos(fai)*cos(omi);R=[a1,b1,c1;a2,b2,c2;a3,b3,c3]forn=1:1:4s1=[Xd(n)-Xs,Yd(n)-Ys,Zd(n)-Zs]';X_(n)=[a1,b1,c1]*s1;Y_(n)=[a2,b2,c2]*s1;Z_(n)=[a3,b3,c3]*s1;x(n)=-f*X_(n)/Z_(n);y(n)=-f*Y_(n)/Z_(n);end%以上循环用于求解像空间坐标x(1),y(1)到x(4),y(4)forp=1:1:4a11(p)=1/Z_(p)*(a1*f+a3*x(p));a12(p)=1/Z_(p)*(b1*f+b3*x(p));a13(p)=1/Z_(p)*(c1*f+c3*x(p));a21(p)=1/Z_(p)*(a2*f+a3*y(p));a22(p)=1/Z_(p)*(b2*f+b3*y(p));a23(p)=1/Z_(p)*(c2*f+c3*y(p));a14(p)=y(p)*sin(omi)-(x(p)/f*(x(p)*cos(ka)-y(p)*sin(ka))+f*cos(ka))*cos(omi);a15(p)=-f*sin(ka)-x(p)/f*(x(p)*sin(ka)+y(p)*cos(ka));%计算A矩阵a16(p)=y(p);a24(p)=-x(p)*sin(omi)-(y(p)/f*(x(p)*cos(ka)-y(p)*sin(ka))-f*sin(ka))*cos(omi);a25(p)=-f*cos(ka)-y(p)/f*(x(p)*sin(ka)+y(p)*cos(ka));a26(p)=-x(p);%右片endA=[a11(1),a12(1),a13(1),a14(1),a15(1),a16(1);a21(1),a22(1),a23(1),a24(1),a25(1),a26(1);a11(2),a12(2),a13(2),a14(2),a15(2),a16(2);a21(2),a22(2),a23(2),a24(2),a25(2),a26(2);a11(3),a12(3),a13(3),a14(3),a15(3),a16(3);a21(3),a22(3),a23(3),a24(3),a25(3),a26(3);a11(4),a12(4),a13(4),a14(4),a15(4),a16(4);a21(4),a22(4),a23(4),a24(4),a25(4),a26(4)]lx=xx-x;ly=yx-y;%右片L=[lx(1),ly(1),lx(2),ly(2),lx(3),ly(3),lx(4),ly(4)]'X=inv(A'*A)*A'*Ldx=X(1,1);dy=X(2,1);dz=X(3,1);df=X(4,1);do=X(5,1);dk=X(6,1);Xs=Xs+dx;Ys=dy+Ys;Zs=dz+Zs;fai=fai+df;omi=omi+do;ka=ka+dk;ifall(abs(X)0.0003)break;endendVz=Az*Xz-LzXsz=Xsz+dxzYsz=dyz+YszZsz=dzz+Zszfaiz=faiz+dfzomiz=omiz+dozkaz=kaz+dkzV=A*X-LXs=Xs+dxYs=dy+YsZs=dz+Zsfai=fai+dfomi=omi+doka=ka+dk%前方交会a1z=cos(faiz)*cos(kaz);a2z=-cos(faiz)*sin(kaz);a3z=-sin(faiz);b1z=cos(omiz)*sin(kaz)-sin(omiz)*sin(faiz)*cos(kaz);b2z=cos(omiz)*cos(kaz)+sin(omiz)*sin(faiz)*sin(kaz);b3z=-sin(omiz)*cos(faiz);c1z=sin(omiz)*sin(kaz)+cos(omiz)*sin(faiz)*cos(kaz);c2z=sin(omiz)*cos(kaz)-cos(omiz)*sin(faiz)*sin(kaz);c3z=cos(faiz)*cos(omiz);%求迭代后左片旋转矩阵R1的元素R1=[a1z,a2z,a3z;b1z,b2z,b3z;c1z,c2z,c3z];a1=cos(fai)*cos(ka);a2=-cos(fai)*sin(ka);a3=-sin(fai);b1=cos(omi)*sin(ka)-sin(omi)*sin(fai)*cos(ka);b2=cos(omi)*cos(ka)+si