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

X=[36589.41 37631.08 39100.97 40426.54];Y=[25273.32 31324.51 24934.98 30319.81];Z=[2195.17 728.69 2386.50 757.31];f=153.24; s=0.0; S=0.0;for i=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;end

m=S*1000/s Xs0=0.0; Ys0=0.0;for k=1:4;

Xs0=Xs0+X(k); Ys0=Ys0+Y(k);end

Xs0=Xs0/4;Ys0=Ys0/4;Zs0=m*f;fai0=0;omig0=0;ka0=0;

for u=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=[];

for h=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=[lxh lyh]';

Ah=[a11h,a12h,a13h,a14h,a15h,a16h;a21h,a22h,a23h,a24h,a25h,a26h];A=[A;Ah];L=[L;lh];end

XX=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);

if abs(XX(4))<0.0000291 & abs(XX(5))<0.0000291 & abs(XX(6))<0.0000291 breakendend

R=[a1,a2,a3,;b1,b2,b3;c1,c2,c3]Xs=Xs0Ys=Ys0Zs=Zs0fai=fai0

omig=omig0ka=ka0