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

摄影测量前方交会后方交会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=10000 Zsz=mz*f+1/4*(Zdz(1)+Zdz(2)+Zdz(3)+Zdz(4)); Xz=[1;1;1;1;1;1]; while 1 a1z=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); aiz)*sin(omiz)*sin(kaz);c2z=-sin(faiz)*sin(kaz)+cos(c1z=sin(faiz)*cos(kaz)+cos(f faiz)*sin(omiz)*cos(kaz);c3z=cos(faiz)*cos(omiz); R=[a1z,b1z,c1z;a2z,b2z,c2z;a3z,b3z,c3z] for n=1:1:4 s1z=[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) for p=1:1:4 a11z(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) ; %右片 end Az=[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'*Lz dxz=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; if all(abs(Xz)<0.0003) break; end end f=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+431Sx=sqrt((xx(2)-xx(1))^2+(yx(2)-yx(1))^2); Sd=sqrt((Xd(2)-Xd(1))^2+(Yd(2)-Yd(1))^2); m=10000 Zs=m*f+1/4*(Zd(1)+Zd(2)+Zd(3)+Zd(4)); X=[1;1;1;1;1;1]; while 1 a1=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] for n=1:1:4 s1=[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); -f*Y_(n)/Z_(n); y(n)= end %以上循环用于求解像空间坐标x(1),y(1)到x(4),y(4) for p=1:1:4 a11(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) ; %右片 end A=[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'*L dx=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; if all(abs(X)<0.0003) break; end end Vz=Az*Xz-Lz Xsz=Xsz+dxz Ysz=dyz+Ysz Zsz=dzz+Zsz faiz=faiz+dfz omiz=omiz+doz kaz=kaz+dkz V=A*X-L Xs=Xs+dx Ys=dy+Ys Zs=dz+Zs fai=fai+df omi=omi+do ka=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(o miz)*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(o miz)*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)+sin(omi)*sin(f ai)*sin(ka);b3=-sin(omi)*cos(fai); c1=sin(omi)*sin(ka)+cos(omi)*sin(fai)*cos(ka);c2=sin(omi)*cos(ka)-cos(omi)*sin(f ai)*sin(ka);c3=cos(fai)*cos(omi); %求迭代后右片旋转矩阵R1的元素 R2=[a1,a2,a3;b1,b2,b3;c1,c2,c3]; Bx=Xs-Xsz; By=Ys-Ysz; Bz=Zs-Zsz; xz=[0.051758 0.014618 0.04988 0.086243 0yz=[0.081555 -0.000231 -0.000792 -0.001346 -0.079962]; zz=[-f -f -f -f -f]; x=[-0.039953 -0.0076016 -0.042201 -0.0076706 -0.044438]; y=[0.078463 0.000036 -0.001022 -0.002112 -79.736]; z=[-f -f -f -f -f ]; for p=1:1:5 Xz(p)=a1z*xz(p)+a2z*yz(p)+a3z*zz(p); Yz(p)=b1z*xz(p)+b2z*yz(p)+b3z*zz(p); Zz(p)=c1z*xz(p)+c2z*yz(p)+c3z*zz(p); X2(p)=a1*x(p)+a2*y(p)+a3*z(p); Y2(p)=a1*x(p)+a2*y(p)+a3*z(p); Z2(p)=a1*x(p)+a2*y(p)+a3*z(p); end Xz=[Xz(1),Xz(2),Xz(3),Xz(4),Xz(5);Yz(1),Yz(2),Yz(3),Yz(4),Yz(5);Zz(1),Zz(2),Zz(3 ),Zz(4),Zz(5)] X=[X2(1),X2(2),X2(3),X2(4),X2(5);Y2(1),Y2(2),Y2(3),Y2(4),Y2(5);Z2(1),Z2(2),Z2(3) ,Z2(4),Z2(5)] for p=1:1:5 N(p)=(Bx*X(p+10)-Bz*X(p+10))/(Xz(p)*X(p+10)-X(p)*Xz(p+10)); end N1=[N(1),N(2),N(3),N(4),N(5)] for p=1:1:5 XA(p)=Xsz+N1(p)*Xz(p); YA(p)=Ysz+N1(p)*Xz(p+5); ZA(p)=Zsz+N1(p)*Xz(p+10); end XYZ=[XA(1) XA(2) XA(3) XA(4) XA(5);YA(1) YA(2) YA(3) YA(4) YA(5);ZA(1) ZA(2) ZA(3) ZA(4) ZA(5)]