摄影测量-单像空间后方交会-matlab编程

x=[-86.15-53.40-14.7810.46];y=[-68.9982.21-76.6364.43];X=[36589.4137631.0839100.9740426.54];Y=[25273.3231324.5124934.9830319.81];Z=[2195.17728.692386.50757.31];f=153.24;s=0.0;S=0.0;fori=1:3;j=i+1;sij=sqrt((x(i)-x(j))^2+(y(i)-y(j))^2);Sij=sqrt((X(i)-X(j))^2+(Y(i)-Y(j))^2);s=s+sij;S=S+Sij;endm=S*1000/sXs0=0.0;Ys0=0.0;fork=1:4;Xs0=Xs0+X(k);Ys0=Ys0+Y(k);endXs0=Xs0/4;Ys0=Ys0/4;Zs0=m*f;fai0=0;omig0=0;ka0=0;foru=1:+inf;a1=cos(fai0)*cos(ka0)-sin(fai0)*sin(omig0)*sin(ka0);a2=-cos(fai0)*sin(ka0)-sin(fai0)*sin(omig0)*cos(ka0);a3=-sin(fai0)*cos(omig0);b1=cos(omig0)*sin(ka0);b2=cos(omig0)*cos(ka0);b3=-sin(omig0);c1=sin(fai0)*cos(ka0)+cos(fai0)*sin(omig0)*sin(ka0);c2=-sin(fai0)*sin(ka0)+cos(fai0)*sin(omig0)*cos(ka0);c3=cos(fai0)*cos(omig0);R=[a1,a2,a3,;b1,b2,b3;c1,c2,c3];L=[];A=[];forh=1:4;O=a1*(X(h)-Xs0)+b1*(Y(h)-Ys0)+c1*(Z(h)-Zs0);P=a2*(X(h)-Xs0)+b2*(Y(h)-Ys0)+c2*(Z(h)-Zs0);Q=a3*(X(h)-Xs0)+b3*(Y(h)-Ys0)+c3*(Z(h)-Zs0);x1h=-f*O/Q;%y1h=-f*P/Q;a11h=(a1*f+a3*x(h))/Q;a12h=(b1*f+b3*x(h))/Q;a13h=(c1*f+c3*x(h))/Q;a14h=y(h)*sin(omig0)-(x(h)/f*(x(h)*cos(ka0)-y(h)*sin(ka0))+f*cos(ka0))*cos(omig0);a15h=-f*sin(ka0)-x(h)/f*(x(h)*sin(ka0)+y(h)*cos(ka0));a16h=y(h);a21h=(a2*f+a3*y(h))/Q;a22h=(b2*f+b3*y(h))/Q;a23h=(c2*f+c3*y(h))/Q;a24h=-x(h)*sin(omig0)-(y(h)/f*(x(h)*cos(ka0)-y(h)*sin(ka0))-f*sin(ka0))*cos(omig0);a25h=-f*cos(ka0)-y(h)/f*(x(h)*sin(ka0)+y(h)*cos(ka0));a26h=-x(h);lxh=x(h)-x1h;lyh=y(h)-y1h;lh=[lxhlyh]';Ah=[a11h,a12h,a13h,a14h,a15h,a16h;a21h,a22h,a23h,a24h,a25h,a26h];A=[A;Ah];L=[L;lh];endXX=inv(A'*A)*A'*L;Xs0=XX(1)+Xs0;Ys0=XX(2)+Ys0;Zs0=Zs0+XX(3);fai0=fai0+XX(4);omig0=omig0+XX(5);ka0=ka0+XX(6);ifabs(XX(4))0.0000291&abs(XX(5))0.0000291&abs(XX(6))0.0000291breakendendR=[a1,a2,a3,;b1,b2,b3;c1,c2,c3]Xs=Xs0Ys=Ys0Zs=Zs0fai=fai0omig=omig0ka=ka0