MATLAB实现单像空间后方交会编程

f=0.15;t=0;w=0;k=0;H=3000;
Xd=load('Xd.txt');
Yd=load('Yd.txt');
Zd=load('Zd.txt');

xx=load('xx.txt');
yx=load('yy.txt');

Xs=(Xd(1)+Xd(2)+Xd(3)+Xd(4))/4;
Ys=(Yd(1)+Yd(2)+Yd(3)+Yd(4))/4;

Zs=H+1/4*(Zd(1)+Zd(2)+Zd(3)+Zd(4));
X=[1;1;1;1;1;1];%构造一个矩阵
while 1
a1=cos(t)*cos(k)-sin(t)*sin(w)*sin(k);a2=-cos(t)*sin(k)-sin(t)*sin(w)*cos(k);a3=-sin(t)*cos(w);
b1=cos(w)*sin(k);b2=cos(w)*cos(k);b3=-sin(w);
c1=sin(t)*cos(k)+cos(t)*sin(w)*sin(k);c2=-sin(t)*sin(k)+cos(t)*sin(w)*cos(k);c3=cos(t)*cos(w);
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);
y(n)=-f*Y_(n)/Z_(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(w)-(x(p)/f*(x(p)*cos(k)-y(p)*sin(k))+f*cos(k))*cos(w);
    a15(p)=-f*sin(k)-x(p)/f*(x(p)*sin(k)+y(p)*cos(k));                  %计算A矩阵     
    a16(p)=y(p);
    a24(p)=-x(p)*sin(w)-(y(p)/f*(x(p)*cos(k)-y(p)*sin(k))-f*sin(k))*cos(w);
    a25(p)=-f*cos(k)-y(p)/f*(x(p)*sin(k)+y(p)*cos(k));  
    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;
t=t+df;
w=w+do;
k=k+dk;
if all(abs(X)<0.001)
    break;
end
end
V=A*X-L
Xs=Xs+dx
Ys=dy+Ys 
Zs=dz+Zs
t=t+df
w=w+do
k=k+dk